perm filename INTERP.PAL[AL,HE]7 blob
sn#337541 filedate 1978-02-26 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00029 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00004 00002 .SBTTL Interpreter Data structures
C00010 00003 INTINIT, NEWENV, MINTS
C00013 00004 Interpreter itself: INTERP
C00019 00005 GETARG, GETSCA, GETVEC, GETTRN
C00024 00006 Variable declaration: MVAR, KVAR
C00027 00007 Stack ops: GTVAL, IGTVAL, CHNGE, ICHNGE, PUSH, POP, COPY, REPLACE, FLUSH
C00033 00008 Global reference routines GLBLNK, GLOBSR.
C00038 00009 Flow-of-control: PROC, RETURN, ABORT, GODDT
C00045 00010 FORCHK, JUMP, JUMPC
C00049 00011 SPAWN, SPROUT, TERMINATE
C00057 00012 Calculator routines: MEXP, MCLC, DCLC, ENDCLC
C00062 00013 Changer routines: MCHG, GTOLD, GTNEW
C00065 00014 Booleans: SLE,SLT,SGE,SGT,SEQ,SNE,AND,LOR,NOT
C00070 00015 return scalars: SADD, SSUB, SMUL, SDIV, SNEG, VDOT, PVDOT, VMAG, SSBRTN
C00078 00016 Vector utilities: UNITV, CROSV
C00084 00017 TRANS extraction routines: TPOS, TORIEN, TAXIS, TMAGN
C00092 00018 Return vectors: SVMUL, TVMUL, VMAKE, VADD, VSUB
C00097 00019 Return a trans: TMAKE, TVADD, TVSUB, TTMUL, TINVRT, VSAXWR
C00109 00020 Motion: MOVE, CENTER
C00114 00021 STOP, TABOFS, WHERE, NOTICE
C00120 00022 Condition monitors: CMMAK
C00129 00023 CMENBL, CMDSBL, CMDEST, CMTRIG, CMSKED, CMUNCR
C00138 00024 CMDONE, CMDUR, CMFORCE, CMSENSE, COMPLY, CMPOFF, VMKFRC, TFRCST
C00154 00025 Events: MAKEVT, SIGNAL, WAITE, DESEVT, PAUSE
C00159 00026 Output routines: PRINT, VALPRN, VARPRN, TACKVAL, TYPVAL, CVFX
C00165 00027 BREAK, NOOP, TOPAL
C00167 00028 Initialization psops: PROG, ENDP, FIXIT
C00173 00029 BUGS
C00174 ENDMK
C⊗;
.SBTTL Interpreter ;Data structures
COMMENT ⊗
Register uses in the interpreter:
R5 used by some routines as the display register
R4 points to interpreter status block
R3 interpreter stack pointer
R2 not used by the main interpreter loop. Can be munged by
any primary interpreter routine.
Each interpreter has a stack which it uses to store pointers to
currently "open" variables. During the course of a calculation,
operands and temporary result cells will be open in this fashion.
The "interpreter stack" is pointed to by R3. When a new interpreter
is sprouted, it is given a new stack area. Each interpreter has
certain status information which facilitates transfer of control
between interpreters. This information is kept in the interpreter
status block, which is always pointed to by R4. Most important are
the IPC, the Interpreter Program Counter, the ENV, which points to
the local environment, and LEV, which stores the current lexical
level.
Each procedure has an environment, which is a data area holding
information vital to that procedure. This includes pointers to all
the variables local to that procedure, and return information.
The environments are administered under the small block allocator
with garbage collection.
⊗
INSTSZ == 20 ;Size of an interpreter stack
;Interpreter status block
II == 0
XX IPC ;Interpreter program counter. Leave this as first field!
XX NXTINT ;Next interpreter in the list. For GC of the stacks.
XX STKBAS ;Location of start of stack area. Needed
;for eventual reclamation.
XX ENV ;Location of local environment
XX LEV ;Lexical level of current execution
XX STA ;Status bits for condition codes: 0 means all well.
XX PDB ;Location of process descriptor block (for reclamation)
XX EVT ;The event to signal as this interpreter goes away
XX CMCB ;Pointer to c-m control block if this is a checker or a body
XX OLDV ;The "old value" used by changers
XX NEWV ;The "new value" used by changers
.IFNZ ALAID ;Special debugging information
XX INTNAM ;Name of the interpreter
XX INTMA1 ; two words
XX DEBMOD ;The mode bits for debugging.
ALDSS == 1 ;1 => Single step mode
ASDTE == 2 ;1 => Terminate this interpeter
XX WAKEVT ;Event to wait on during halts
.ENDC
ISBS == II/2 ;Size (in words) of interpreter status block
;Fixed fields in the environment of each process
II == 0
XX SLINK ;Pointer to environment of next (outer, lower
; numbered) block
XX OLEV ;Old level. The lexical level of calling process.
XX OENV ;Old environment, the one for the calling process.
XX OIPC ;Old IPC. Program counter for calling process.
XX LVARS ;First location where pointers to local variables go
; Mechanism bits.
YARM == 1
YHAND == 2
BARM == 4
BHAND == 10
ANARM == YARM + BARM
AHAND == YHAND + BHAND
; Servo bits.
YARMSB == 176000
YHANDSB == 1000
BARMSB == 770
BHANDSB == 4
; Table offsets for various mechanisms.
OFYARM == 0
OFYHAND == 6*2
OFBARM == 7*2
OFBHAND == 15*2
; Environment offsets for the various mechanisms
.IFNZ YELLOW
YAOFST == 10
YHOFST == 12
.ENDC
BAOFST == 14
BHOFST == 16
; Environment offsets for the calculators of those mechanisms
.IFNZ YELLOW
YACOFS == 20
YHCOFS == 22
BACOFS == 24
BHCOFS == 26
.IFF
BACOFS == 10
BHCOFS == 12
.ENDC
; Environment offsets for the deproach variables
BDEPROACH == 30
YDEPROACH == 32
;INTINIT, NEWENV, MINTS
DATA
INTEVT: 0 ;The event that interlocks references to ISTBLK.
GLBEVT: 0 ;The event that interlocks references to GLBTAB.
CODE
INTINIT: ;Initializes the above events
EVMAK ;Initialize the INTEVT.
MOV (SP),INTEVT;
EVSIG ;
EVMAK ;Initialize the GLBEVT.
MOV (SP),GLBEVT ;
EVSIG ;
MOV #GLBTAB,GLBEND ;Initialize GLBEND. This wipes out all globals.
RTS PC ;Done
MINTS: ;Marking method for interpeters
MOV R2,-(SP) ;Save R2
MOV R3,-(SP) ;Save R3
EVWAIT INTEVT ;Enter critical region
MOV NXTINT+ISTBLK,R2 ;R2 ← LOC[first real interpeter status block]
BEQ 6$ ;If none, then done
;mark the stack
1$: MOV STKBAS(R2),R3 ;R3 ← LOC[interpreter stack base]
ADD #2*INSTSZ,R3 ;R3 ← LOC[verge of new stack] (INSTSZ is in bytes)
2$: MOV -(R3),R0 ;R0 ← stack entry
BEQ 3$ ;If 0, then end of stack (RF: this wont work!!)
JSR PC,MARKQ ;
MOV R0,(R3) ;Put it back (compacting may move it)
BR 2$ ;
;Mark the old & new values used by changers
3$: MOV OLDV(R2),R0
JSR PC,MARKQ ;
MOV R0,OLDV(R2)
MOV NEWV(R2),R0
JSR PC,MARKQ ;
MOV R0,NEWV(R2)
;mark the environments
MOV ENV(R2),R0 ;R0 ← environment
JSR PC,MARKQ ;
MOV R0,ENV(R2) ;
4$: MOV R0,R3 ;
MOV SLINK(R3),R0 ;R0 ← next environment
BEQ 5$ ;if any
JSR PC,MARKQ ;
MOV R0,SLINK(R3) ;
BR 4$
5$: MOV NXTINT(R2),R2 ;R2 ← LOC[next interpreter status block]
BNE 1$ ;Repeat as necessary
6$: MOV (SP)+,R3 ;Restore R3
MOV (SP)+,R2 ;Restore R2
EVSIG INTEVT ;
RTS PC ;Return
NEWENV: ;Gets a new environment, returns address in R0.
.IFNZ SMALLB
MOV #ENVSPC,R0 ;
JMP GETSBK ;Allocate from small blocks
.IFF
MOV #ENVSIZ,R0 ;
JMP GTFREE ;Allocate from large blocks
.ENDC
;Interpreter itself: INTERP
.MACRO MAKEOP CNAME, ANAME ;Compiler name, Address name
XX CNAME
ANAME
.ENDM
DATA
;The interpreter operation table
INTOPS: MAKEOP XINVALID,INVALID ;Illegal instruction
.INSRT INTOPS.PAL[AL,HE]
INSEND = II ;Marks the end of the instructions
CODE
.MACRO BMPIPC ;
ADD #2,IPC(R4) ;Bump IPC
.ENDM ;
.MACRO BACKIPC ;
SUB #2,IPC(R4) ;Backup IPC
.ENDM ;
.MACRO CCC ;Clear condition code
; CLR R0 ;Clear condition code. Not used right now.
.ENDM
.MACRO SCC ;Set condition code
; MOV #2,R0 ;Set condition code. Not used right now. (maybe use TST PC)
.ENDM
INTERP:
MOV R3,R0 ;Save the limit of the interpreter stack for error checking.
SUB #2*INSTSZ,R0
MOV R0,-(SP) ;
INT1: CMP R3,(SP) ;Interpreter stack overflow?
BGE 1$ ;No. Go to next instruction.
ALERR INTMS3 ;Yes. Complain.
1$: CLR -2(R3) ;Zero above top of stack - to keep MINTS happy
MOV @IPC(R4),R0 ;R0 ← next instruction
BLE INVALID ;Instruction out of range
CMP R0,#INSEND ;Is instruction too large?
BLE INT2 ;No.
INVALID:ALERR INTMS1 ;Yes. complain.
INT2: BMPIPC ;Bump IPC
JSR PC,@INTOPS(R0) ;Call the appropriate routine
BR INT1 ;Repeat interpreter loop
DATA
INTMS1: ASCIE /INTERPRETER INSTRUCTION OUT OF RANGE/
INTMS2: ASCIE /INTERPRETED INSTRUCTION RETURNED FAILURE/
INTMS3: ASCIE /INTERPRETER STACK OVERFLOW/
CODE
; GETARG, GETSCA, GETVEC, GETTRN
GETARG:
COMMENT ⊗
Arguments:
R0=variable name: high byte is lexical level, low byte is offset.
R4=pointer to interpreter status block.
Result:
R0← pointer to address of desired variable.
R1 clobbered.
This routine returns in R0 a pointer to the location in the current
environment (or, if necessary, more global environment) which
points to the variable which is named in R0. ⊗
MOV R2,-(SP) ;Save R2
MOV R0,R1 ;
BIC #177400,R1 ;R1 ← Offset desired
CLRB R0 ;
SWAB R0 ;R0 ← Lexical level
MOV ENV(R4),R2 ;R2 ← LOC[local environment]
SUB LEV(R4),R0 ;R0 ← Difference in levels: desired-got
BEQ 2$ ;Diff=0; can use R2 as pointer at right base.
BHI 3$ ;If diff>0, then value inaccessible.
1$: MOV SLINK(R2),R2;Must go up a level. R2 ← LOC[more global environment]
INC R0 ;R0 ← New difference in levels
BNE 1$ ;If not yet good, then move up another level
2$: ADD R2,R1 ;R1 ← environment + offset = location of desired pointer
MOV (SP)+,R2 ;Restore R2.
MOV R1,R0 ;
RTS PC ;Done.
3$: PUNT GTMS1
DATA
GTMS1: ASCIE /ATTEMPT TO ACCESS UNAVAILABLE VARIABLE/
CODE
GETSCA: ;Gets place for a scalar result, and places a pointer on
;the interpreter stack. Location is returned in R0.
;Simple procedure.
.IFNZ SMALLB
MOV #SCASPC,R0 ;
JSR PC,GETSBK ;Allocate from small blocks
MOV R0,-(R3) ;Push new value cell pointer on interpreter stack.
EVSIG SBEVT ;End of critical section
.IFF
MOV #2,R0 ;Number of words needed
JSR PC,GTFREE ;R0 ← LOC[new block]
MOV R0,-(R3) ;Push new value cell pointer on interpreter stack.
.ENDC
RTS PC ;Done
GETVEC: ;Gets place for a vector result, and places a pointer on
;the interpreter stack. Location is returned in R0.
;Simple procedure.
.IFNZ SMALLB
MOV #VCTSPC,R0 ;
JSR PC,GETSBK ;Allocate from small blocks
MOV R0,-(R3) ;Push new value cell pointer on interpreter stack.
EVSIG SBEVT ;End of critical section
.IFF
MOV #10,R0 ;Number of words needed
JSR PC,GTFREE ;R0 ← LOC[new block]
MOV R0,-(R3) ;Push new value cell pointer on interpreter stack.
.ENDC
RTS PC ;Done
GETTRN: ;Gets place for a trans result, and places a pointer on
;the interpreter stack. Location is returned in R0.
;Simple procedure.
.IFNZ SMALLB
MOV #TRNSPC,R0 ;
JSR PC,GETSBK ;Allocate from small blocks
MOV R0,-(R3) ;Push new value cell pointer on interpreter stack.
EVSIG SBEVT ;End of critical section
.IFF
MOV #40,R0 ;Number of words needed
JSR PC,GTFREE ;R0 ← LOC[new block]
MOV R0,-(R3) ;Push new value cell pointer on interpreter stack.
.ENDC
RTS PC ;Done
;Variable declaration: MVAR, KVAR;
MVAR:
COMMENT ⊗ A list of arguments, each of which is an offset. This list
is terminated by a zero entry. For each argument, a fresh graph node
is created (with no value) and a pointer to it is placed in the
environment at the desired offset, current level. ⊗
MOV @IPC(R4),-(SP) ;push offset
BIC #177400,(SP);Get rid of level info.
BEQ 1$ ;If none, done
BMPIPC ;Bump IPC
CLR R0 ;The new graph node should get no value cell.
JSR PC,MAKEVN ;R0 ← LOC[a new variable node]
ADD ENV(R4),(SP);stack pointer into environment
MOV R0,@(SP)+ ;Point the environment to the graph node
BR MVAR ;Repeat
1$: TST (SP)+ ;Clean off stack
BMPIPC ;Bump IPC
CCC ;Clear condition code.
RTS PC ;Done
KVAR:
COMMENT ⊗ A list of arguments, each of which is an offset. This list
is terminated by a zero entry. For each argument, the corresponding
graph node is destroyed in the current environment. Any function in
the graph structure is thereby released. (Attempt is made to
validate any dependents first.) ⊗
MOV @IPC(R4),R2 ;R2 ← offset
BIC #177400,R2 ;Get rid of level info.
BEQ 1$ ;If none, done
BMPIPC ;Bump IPC
ADD ENV(R4),R2 ;R2 ← LOC[pointer at graph node]
MOV (R2),R0 ;R0 ← LOC[graph node]
JSR PC,DELVN ;Get this guy deleted
CLR (R2) ;Remove the pointer in the environment
BR KVAR ;Repeat
1$: BMPIPC ;Bump IPC
CCC ;Clear condition code
RTS PC ;Done
;Stack ops: GTVAL, IGTVAL, CHNGE, ICHNGE, PUSH, POP, COPY, REPLACE, FLUSH
GTVAL:
COMMENT ⊗ The argument is a level-offset pair. The variable
referenced by that pair is examined and a pointer to its value cell
is placed on the stack. ⊗
MOV @IPC(R4),R0 ;Pick up level-offset name of argument
BMPIPC ;Bump IPC
JSR PC,GETARG ;R0 ← LOC[LOC[desired graph node]]
MOV (R0),R0 ;R0 ← LOC[desired graph node]
BEQ 4$ ;But if 0, then bug
JSR PC,NOCMP ;Don't compact for a bit
1$: CALL GETVAL,<R0>;R0 ← value
MOV R0,-(R3) ;Push value on interpreter stack.
JSR PC,YESCMP ;OK to compact now
TST (R3)
BEQ 3$ ;But if 0, then bug
CCC ;Clear condition code.
RTS PC ;Done
3$: PUNT GTVMS1 ;Complain
2$: SCC ;Set condition code
RTS PC ;Done
4$: PUNT GTVMS2 ;Complain
BR 2$
DATA
GTVMS1: ASCIE </GTVAL FOUND A NULL VALUE./>
GTVMS2: ASCIE </GTVAL FOUND A NULL GRAPH NODE./>
CODE
IGTVAL:
COMMENT ⊗ Immediate version of GTVAL. The argument points directly
to the graph node whose value is desired. A pointer to the value
cell is placed on the stack. ⊗
MOV @IPC(R4),R0 ;R0 ← LOC[desired graph node]
BMPIPC ;Bump IPC
JSR PC,NOCMP ;Don't compact for a bit
CALL GETVAL,<R0>;R0 ← value
MOV R0,-(R3) ;Push value on interpreter stack.
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code.
RTS PC ;Done
CHNGE:
COMMENT ⊗ Pops the value from top of stack into the graph structure
pointed to by the level-offset pair given in the argument. ⊗
MOV @IPC(R4),R0 ;Pick up level-offset name of argument
BMPIPC ;Bump IPC
JSR PC,GETARG ;R0 ← LOC[LOC[Desired graph node]]
MOV (R0),R0 ;R0 ← LOC[Desired graph node]
BEQ 1$ ;If any
JSR PC,NOCMP ;Don't compact for a bit
CALL CHANGE,<R0,(R3)>
JSR PC,YESCMP ;OK to compact now
TST (R3)+ ;Pop stack
CCC ;Clear condition code.
RTS PC ;Done
1$: ALERR 2$ ;Complain
TST (R3)+ ;Get rid of the value
SCC ;Set condition code
RTS PC ;Done
DATA
2$: ASCIE </CAN'T ASSIGN INTO UNINITIALIZED VARIABLE/>
CODE
ICHNGE:
COMMENT ⊗ Immediate version of CHNGE. Pops the value from top of
stack into the graph structure pointed to directly by the argument. ⊗
MOV @IPC(R4),R0 ;R0 ← LOC[desired graph node]
BMPIPC ;Bump IPC
JSR PC,NOCMP ;Don't compact for a bit
CALL CHANGE,<R0,(R3)>
JSR PC,YESCMP ;OK to compact now
POP: TST (R3)+ ;Pop stack
CCC ;Clear condition code.
RTS PC ;Done
PUSH: MOV @IPC(R4),-(R3);Put argument directly on stack
BMPIPC ;Bump IPC
CCC ;Clear condition code.
RTS PC ;Done
; Interpreter routine. Copies the nth element in stack to the top,
; where the curent top is 0.
COPY: MOV @IPC(R4),R0 ;Pick up argument.
BMPIPC ;Bump IPC
ADD R0,R0 ;Double R0 to make it in bytes
ADD R3,R0 ;R0 ← LOC[stack element to be copied to top]
MOV (R0),-(R3) ;Copy it onto top of stack.
CCC ;Clear condition code.
RTS PC ;Done
REPLAC: MOV @IPC(R4),R0 ;Pick up argument.
BMPIPC ;Bump IPC
ADD R0,R0 ;Double R0 to make it in bytes
ADD R3,R0 ;R0 ← LOC[stack element to be copied into]
MOV (R3)+,(R0) ;Copy verge of stack into it.
CCC ;Clear condition code.
RTS PC ;Done
FLUSH: MOV STKBAS(R4),R3;Reset the stack base.
CCC ;Clear condition code.
RTS PC ;Done
;Global reference routines GLBLNK, GLOBSR.
GLBLNK: ;Interpreter routine
COMMENT ⊗ Expects two arguments at the IPC, a level-offset, and two
words of a Rad50 name. Makes sure that this global is linked in to
the environment at the given level-offset. If not, a search is
made for it, and the result is put in the environment.
⊗
MOV @IPC(R4),R0 ;R0 ← level-offset
BMPIPC ;Bump IPC past the level-offset
JSR PC,GETARG ;R0 ← LOC[environment cell]
TST (R0) ;Graph node yet?
BEQ 2$ ;No, must search for it
1$: BMPIPC ;Bump IPC past the Rad50 name
BMPIPC ;Bump IPC past the Rad50 name
RTS PC ;Done
2$: MOV R0,R2 ;R2 ← LOC[environment cell]
MOV IPC(R4),R0 ;R0 ← LOC[Rad50 representation]
JSR PC,GLOBSR ;R0 ← LOC[new or old graph node]
MOV R0,(R2) ;Stow LOC[graph node] in the environment cell
BR 1$ ;Ready to return
DATA
MAXGLB == 10 ;Maximum number of globals allowed
GLBTAB: .BLKW 3*MAXGLB ;Three words per global: 2 of Rad50, one
;pointer to the graph node.
;To be searched linearly.
GLBLIM: .BLKW 3 ;Overflow place for GLBTAB
GLBEND: .BLKW 1 ;Points to next free place in GLBTAB
CODE
GLOBSR:
COMMENT ⊗ R0 = LOC[two words of Rad50]. Tries to find the
appropriate graph node using the GLBTAB, and if it fails, makes a new
graph node and inserts it in the GLBTAB. In any case, returns R0 ←
LOC[new or old graph node]. ⊗
EVWAIT GLBEVT ;Critical region starts here
MOV GLBEND,R1 ;R1 ← LOC[next free place in GLBTAB]
MOV (R0),(R1)+ ;Put the word sought at next free place
MOV 2(R0),(R1)+ ;
CLR (R1) ; with a 0 for a graph node pointer.
MOV #GLBTAB,R1 ;R1 ← LOC[start of GLBTAB]
1$: CMP (R0),(R1) ;MATCH?
BNE 2$ ;No.
CMP 2(R0),2(R1) ;Second word match?
BEQ 3$ ;Yes.
2$: ADD #6,R1 ;
BR 1$ ;Try again.
3$: MOV 4(R1),R0 ;R0 ← LOC[graph node]
BNE 5$ ;If it is not zero, we are done
ADD #6,GLBEND ;Move the end of the table down one entry
CMP GLBEND,#GLBLIM ;Too far?
BLT 4$ ;No
ALERR GLOBMS ;Yes
4$: MOV R1,-(SP) ;Save place in GLBTAB
CLR R0 ;New graph node should have no value cell.
JSR PC,MAKEVN ;R0 ← LOC[a new variable node]
MOV (SP)+,R1 ;Restore place in GLBTAB
MOV R0,4(R1) ;store LOC[new graph node] in GLBTAB
5$: EVSIG GLBEVT ;Critical region ends here
RTS PC ;Done
DATA
GLOBMS: ASCIE </TOO MANY GLOBALS/>
CODE
;Flow-of-control: PROC, RETURN, ABORT, GODDT
PROC:
;Procedure call. Arguments:
; Destination.
; List of variables which are to be inserted in appropriate
; locations in the local storage of procedure. These are
; in the format variable (ie level-offset pair), new offset
; (right justified in the second word).
; There is a zero word to finish these.
;At the destination address can be found:
II == 0
XX FSLGTH ;Number of words to get from free storage
;for local variable pointers
XX PLEV ;Lexical level of procedure
DSLGTH == II ;Number of words before code starts
;Value parameters should have first been copied first into local temps
; (which have been arranged by the compiler), and then the temps are
; passed by reference. Eventual problem: to know which variables to
; really kill as the procedure is exited.
MOV @IPC(R4),R2 ;R2 ← LOC[destination]
BMPIPC ;Bump IPC
MOV FSLGTH(R2),R0 ;R0 ← Number of words to get.
JSR PC,GTFREE ;R0 ← LOC[block with that number of words]
;initialize pointer to lexical level:
MOV PLEV(R2),R1 ;R1 ← Lexical level of procedure
MOV ENV(R4),R2 ;R2 ← LOC[current environment]
SUB LEV(R4),R1 ;R1 ← Difference in levels: desired-got
BEQ 2$ ;Diff=0; can use R2 as pointer at right environment.
1$: MOV SLINK(R2),R2;No, must go up a level. R2 ← LOC[base of upper area]
INC R1 ;R1 ← New difference in levels
BNE 1$ ;If not yet good, then move up another level
2$: MOV R2,SLINK(R0);SLINK[new environment] ← correct global environment
;Put copies of local variables in new area
MOV R0,-(SP) ;Stack LOC[new environment]
MOV @IPC(R4),R0 ;R0 ← level-offset pair for an argument
BEQ 4$ ;If there are no more, go to next phase
3$: BMPIPC ;Else bump IPC
JSR PC,GETARG ;R0 ← LOC[LOC[graph node]]
MOV @IPC(R4),R1 ;R1 ← offset in new block
BMPIPC ;Bump IPC
ADD (SP),R1 ;R1 ← LOC[place in new environment to put pointer]
MOV (R0),(R1) ;new environment gets pointer to LOC[argument graph node]
MOV @IPC(R4),R0 ;R0 ← level-offset pair for an argument
BNE 3$ ;If there are more, go back and treat them
4$: BMPIPC ;Bump IPC one last time
;Save the old context in the new area
MOV (SP)+,R1 ;R1 ← LOC[new environment]
MOV LEV(R4),OLEV(R1) ;Store the old level
MOV ENV(R4),OENV(R1) ;Store the old environment location
MOV IPC(R4),OIPC(R1) ;Store the return address
;Set up the new context for procedure
MOV PLEV(R2),LEV(R4) ;New lexical level
MOV R1,ENV(R4) ;New environment location
ADD #DSLGTH,R2 ;R2 ← Place where execution should begin
MOV R2,IPC(R4) ;New program counter
CCC ;Clear condition code.
RTS PC ;Done
RETURN:
;Returns from a procedure call to calling program. Since variables are
;passed by reference, it is not necessary to do any copying of values.
;All that is needed is to restore the context of the caller and to
;discard the display.
MOV ENV(R4),R0 ;R0 ← LOC[current environment]
MOV OLEV(R0),LEV(R4) ;Restore the old lexical level
MOV OENV(R0),ENV(R4) ;Restore the old environment
MOV OIPC(R0),IPC(R4) ;Restore the IPC
JSR PC,RLFREE ;Release storage of old display
CCC ;Clear condition code.
RTS PC ;Done
ABORT:
;Aborts current motions
;This should be cleaned up sometime.
MOV #16,R1 ;First stop all devices - 2 arms (6 joints/arm) & 2 hands
MOV LDVCPTR,R0 ;R0 ← LOC[table of device pointers]
1$: MOV (R0)+,R2 ;R2 ← device block
BEQ 2$ ;If any
BIS #100000,(R2) ;Stop this device
2$: SOB R1,1$ ;Repeat till all devices stopped
;SLEEP #144 ;Should pause for a bit (1/10 sec) here but...
; if anything gets printed no problem
CCC ;Clear the condition codes
RTS PC ; & Return
GODDT: BPT ;break to DDT
CCC ;Clear the condition codes
RTS PC ; and Return
; FORCHK, JUMP, JUMPC
FORCHK:
;Assume that the stack has, from surface in, the increment, the
; final value, and the control variable's value, all of which are
; scalar values. If (FINAL-CONVAR)*(INCREMENT) ≥ 0 then this is a
; no-op; otherwise, jump to the destination.
;Arguments: destination.
LDF @2(R3),AC0 ;AC0 ← final value
SUBF @4(R3),AC0 ;AC0 ← final - current
MULF @(R3),AC0 ;AC0 ← (final - current)*increment
MOV @IPC(R4),R0 ;R0 ← destination
BMPIPC ;Bump IPC
CFCC ;
BGE 1$ ;Shall this be a no-op?
MOV R0,IPC(R4) ;No; set new IPC.
1$: CLR R0 ;
RTS PC ;Done
JUMP:
;Takes one argument: the new address.
MOV @IPC(R4),IPC(R4)
CCC ;Clear condition code.
RTS PC ;Done
JUMPC: ;Interpreter routine
COMMENT ⊗ Takes one argument: the destination address.
The condition queries the top of the stack and pops it, assuming it
to be a scalar. The interpreter jumps to the destination address if
the value of the scalar is false (0). rewritten 9-14-76 by arg ⊗
LDF @(R3)+,AC0 ;Get value of boolean
CFCC ;copy condition codes
BEQ 1$ ;if false succeed - take branch
BMPIPC ;skip over address
RTS PC ; & return
1$: MOV @IPC(R4),IPC(R4); branch
RTS PC ; & return
; SPAWN, SPROUT, TERMINATE
SPAWN: ;Utility routine
COMMENT ⊗ Takes two arguments: In R0, the IPC of the interpreter to
spawn, and in R1, the event (if any) to put in EVT of the new
interpreter. The inferior will have the same environment as the
superior. Creates an interpreter status block, stack, process
descriptor, and is ready for a SCHEDU when it returns the process
descriptor in R0. ⊗
MOV R1,-(SP) ;Save the EVT
MOV R0,-(SP) ;Save the new IPC
MOV #ISBS,R0 ;R0 ← Size (in words) of an interpreter status block
JSR PC,GTFREE ;R0 ← LOC[new interpreter status block]
MOV (SP)+,IPC(R0) ;new IPC ← first argument
MOV ENV(R4),ENV(R0) ;new ENV ← old ENV
MOV LEV(R4),LEV(R0) ;new LEV ← old LEV
.IFNZ ALAID
MOV DEBMOD(R4),DEBMOD(R0) ;new DEBMOD ← old DEBMOD
.ENDC
EVWAIT INTEVT ;Interlock sensitive operation.
MOV #NXTINT+ISTBLK,R1 ;Link into the interpreter list.
MOV (R1),NXTINT(R0)
MOV R0,(R1)
EVSIG INTEVT ;End of interlock
MOV (SP)+,EVT(R0) ;new EVT ← second argument.
MOV R0,-(SP) ;Save LOC[new interpreter status block]
MOV #INSTSZ,R0 ;R0 ← Size needed for an interpreter stack
JSR PC,GTFREE ;R0 ← LOC[new interpreter stack]
MOV (SP)+,R1 ;R1 ← LOC[new interpreter status block]
MOV R0,STKBAS(R1) ;Store away new stack base
ADD #2*INSTSZ,R0 ;R0 ← LOC[top of new stack] (INSTSZ is in bytes)
MOV R1,-(SP) ;Save R1
MOV R0,-(SP) ;Save R0
MOV #210,R0 ;Room for process descriptor
JSR PC,GTFREE ;R0 ← LOC[new process descriptor]
MOV #UFPUSE+UGRSAV+2,PDBSTA(R0) ;Use floating point,saved registers, pri=1
MOV (SP)+,PDBR3(R0) ;Store away new interp stack pointer (reg 3)
MOV (SP)+,R1 ;R1 ← LOC[new ISB]
MOV R0,PDB(R1) ;Store away LOC[PDB] in new ISB
MOV R1,PDBR4(R0) ;Store away LOC[ISB] in reg 4 of PDB
MOV R0,USKMIN(R0) ;Set up min pointer for SP
ADD #UFEC+36,USKMIN(R0)
MOV R0,USKMAX(R0) ;Set up max pointer for SP
ADD #420,USKMAX(R0)
MOV #144040,UPSW(R0) ;Set up psw
MOV PDB(R4),R1 ;Use same UIMAP that we are using.
MOV UIMAP(R1),UIMAP(R0)
RTS PC ;Done
; These are the appropriate scheduling commands:
; SCHEDU R0,#INTERP,#USRDM,#2 ;Cause the new process to be started, suspended
; FORK R0,#INTERP,#USRDM ;Cause the new process to be started.
SPROUT: ;Interpreter routine
COMMENT ⊗ Arguments: One address in pseudo-code for each of the
several forks starting up, followed by a 0 word. This is to be used
only for cobegins, not for servos. Each new interpreter is given an
interpreter status block and is then scheduled. As each terminates,
it signals its defining event. Since each of these has the same
event, the current interpreter need only wait until they all happen.
⊗
MOV R3,-(SP) ;Save R3. Caution: cannot use interpreter stack now.
CLR R3 ;R3 is the count of how many inferiors to spawn.
EVMAK ;-(SP) ← Event identifier for communication with infs.
1$: MOV @IPC(R4),R0 ;R0 ← next argument (IPC)
BEQ 2$ ;If zero, then we have spawned all the inferiors.
BMPIPC ;Bump IPC
INC R3 ;Count it.
MOV (SP),R1 ;R1 ← event for the inferior EVT
JSR PC,SPAWN ;
MOV R0,R2 ;R2 ← new process control block
;Set up the new environment
JSR PC,NEWENV ;R0 ← LOC[new environment]
MOV ENV(R4),SLINK(R0) ;Not necessary to set up OLEV, etc.
MOV PDBR4(R2),R1;
MOV R0,ENV(R1) ;
.IFNZ SMALLB
EVSIG SBEVT ;End of critical section - value stored
.ENDC
INC LEV(R1) ;
SCHEDU R2,#INTERP,#USRDM,#2 ;Cause the new process to be started, suspended
BR 1$ ;Go handle the next inferior.
2$: BMPIPC ;Bump IPC
3$: DEC R3 ;Another wait to be done?
BMI 4$ ;No, we are finished.
EVWAIT (SP) ;Wait for an inferior to come back.
BCC 3$ ;If all well, wait for the next one.
ALERR SPRMES ;The event was killed!
4$: EVKIL (SP)+ ;Kill the event now, remove from stack
MOV (SP)+,R3 ;Restore R3
CCC ;Clear condition code.
RTS PC ;Done
DATA
SPRMES: ASCIE /BAD RETURN FROM INFERIOR/
CODE
TERMINATE:
COMMENT ⊗ Interpreter routine, sometimes jumped to from other
interpreter routines. End this interpreter. ⊗
MOV EVT(R4),R0 ;R0 ← event to announce imminent demise
BEQ 1$ ;If there is one
EVSIG R0 ;Announce that we are about to disappear.
1$: MOV STKBAS(R4),R0 ;Reclaim interpreter stack
JSR PC,RLFREE
MOV PDB(R4),-(SP) ;Save LOC[this PDB]
MOV R4,R0 ;Reclaim Interpreter Status Block
JSR PC,RLFREE
EVWAIT INTEVT ;Enter critical region.
MOV #ISTBLK,R0 ;The following unlinks this interpreter from the chain.
2$: MOV R0,R1
MOV NXTINT(R1),R0
CMP R0,R4 ;Have we found ours yet?
BNE 2$
MOV NXTINT(R4),NXTINT(R1) ; Yes. rechain.
EVSIG INTEVT ;Leave critical region.
MOV (SP)+,R0 ;Reclaim process control block (should be safe now)
CMP R0,#FREEST ;Make sure that it points into free storage.
BLE 3$ ; (it may be statically allocated)
CMP R0,#FREEND
BGE 3$
JSR PC,RLFREE
3$: DISMIS ;Go away
;Calculator routines: MEXP, MCLC, DCLC, ENDCLC;
COMMENT ⊗ Make an expression, put it in enviroment. Arguments are
the needed list (level-offset list, terminated by 0), the IPC
(ablsolute address), and the offset. ⊗
MEXP: ;Interpreter routine.
JSR PC,NOGC ;Can't allow garbage collection til the needed
; list is stored away
;Form the needed list
CLR -(SP) ;Start with null needed list on the stack
1$: MOV @IPC(R4),R0 ;R0 ← the next needed level-offset
BEQ 2$ ;Any more?
BMPIPC ;Bump IPC
JSR PC,GETARG ;R0 ← LOC[LOC[next needed graph node]]
MOV (R0),-(SP) ;Stack next needed graph node
JSR PC,NEWCEL ;R0 ← LOC[new cell]
MOV (SP)+,CAR(R0) ;LOC[Needed graph node]
MOV (SP),CDR(R0);Link to rest of needed list
MOV R0,(SP) ;New needed list
.IFNZ SMALLB
EVSIG SBEVT ;End of critical section - value stored (sort of)
.ENDC
BR 1$ ;Repeat
2$: BMPIPC ;Bump IPC past the 0 at end of list
MOV (SP)+,R0 ;R0 ← needed list
MOV @IPC(R4),R1 ;R1 ← IPC
BMPIPC ;Bump IPC
CALL MAKEXP,<R4,R1,R0> ;R0 ← LOC[new expression node]
MOV @IPC(R4),R1 ;R1 ← offset
BMPIPC ;Bump IPC
BIC #177400,R1 ;Remove level info.
ADD ENV(R4),R1 ;R0 ← Pointer into environment
MOV R0,(R1) ;Stow away pointer to expression node
JSR PC,YESGC ;Garbage collection okay now
CCC ;Clear condition code
RTS PC ;Done
MCLC: ;Interpreter routine.
COMMENT ⊗ Takes two arguments: the level-offset of the expression,
and the level-offset of the variable for which this expression is to
be a calculator. ⊗
MOV @IPC(R4),R0 ;R0 ← Level-offset of expression
BMPIPC ;Bump IPC
JSR PC,GETARG ;R0 ← LOC[LOC[exression node]]
MOV (R0),R2 ;R2 ← LOC[expression node]
MOV @IPC(R4),R0 ;R0 ← level-offset of variable
BMPIPC ;Bump IPC
JSR PC,GETARG ;R0 ← LOC[LOC[variable node]]
CALL ADDCLC,<(R0),R2> ;Do the linking
CCC ;Clear condition code
RTS PC ;Done
DCLC: ;Interpreter routine
COMMENT ⊗ Takes two arguments: the level-offset of the expression,
and the level-offset of the variable from which this expression is to
be removed as a calculator. ⊗
MOV @IPC(R4),R0 ;R0 ← Level-offset of expression
BMPIPC ;Bump IPC
JSR PC,GETARG ;R0 ← LOC[LOC[exression node]]
MOV (R0),R2 ;R2 ← LOC[expression node]
MOV @IPC(R4),R0 ;R0 ← level-offset of variable
BMPIPC ;Bump IPC
JSR PC,GETARG ;R0 ← LOC[LOC[variable node]]
CALL REMCLC,<(R0),R2> ;Do the unlinking
CCC ;Clear condition code
RTS PC ;Done
ENDCLC: ;Interpreter routine.
COMMENT ⊗ Called as last instruction in a calculator cell. Returns
via an RTS RF with the value from the top of the stack in R0. Does
not unlink anything. ⊗
MOV (R3)+,R0 ;Get the coveted value cell
MOV RF,SP ;Reset the stack
MOV -2(SP),RF ;RF ← old PC
RTS RF ;Just return
;Changer routines: MCHG, GTOLD, GTNEW
COMMENT ⊗ Make a changer for a graph node. This involves several
data: the target variable, specified as a level-offset pair, and the
location of the changer code, (which is ordinary interpreter code
which terminates with TERMINATE). These data are passed as arguments
to MCHG: target (level-offset), IPC (absolute address). Recall that
a changer cell looks like this:
II==0
XX NXTCHG ;next changer cell in chain
XX CHGISB ;Points to interpreter status block to resolve addressing
XX CHGIPC ;the interpeter PC where the calculation starts
CHGCSZ == II/2 ;Size of changer cell, in words
⊗
MCHG: ;Interpreter routine.
MOV R2,-(SP) ;Save R2
MOV R3,-(SP) ;Save R3
MOV #CHGCSZ,R0 ;Get room for a changer cell
JSR PC,GTFREE ;Note that we use large block allocation
MOV R0,R3 ;R3 ← LOC[new changer cell]
MOV R4,CHGISB(R3) ;store away ISB
MOV @IPC(R4),R0 ;R0 ← level-offset pair.
BMPIPC ;Bump IPC
JSR PC,GETARG ;R0 ← LOC[LOC[target graph node]]
MOV (R0),R2 ;R2 ← LOC[target graph node]
MOV @IPC(R4),CHGIPC(R3) ;store away target IPC
BMPIPC ;Bump IPC
CALL ADDCHG,<R2,R3> ;Do the final linking
MOV (SP)+,R3 ;Restore R3
MOV (SP)+,R2 ;Restore R2
CCC ;Clear condition code
RTS PC ;Done
GTOLD: ;Interpreter routine
COMMENT ⊗ Gets the OLD value that this changer (acting as an
interpreter) has access to; puts it on the stack. ⊗
MOV OLDV(R4),-(R3)
CCC ;Clear condition code
RTS PC ;Done
GTNEW: ;Interpreter routine
COMMENT ⊗ Gets the NEW value that this changer (acting as an
interpreter) has access to; puts it on the stack. ⊗
MOV NEWV(R4),-(R3)
CCC ;Clear condition code
RTS PC ;Done
;Booleans: SLE,SLT,SGE,SGT,SEQ,SNE,AND,LOR,NOT
COMP: ;auxiliary function used by SLE,SLT,SGE,SGT,SEQ,SNE
LDF @(R3)+,AC0 ;Get first arg
CMPF @(R3)+,AC0 ;Compare it with second arg (1st-2nd)
JSR PC,NOCMP ;Don't compact for a bit
JSR PC, GETSCA ;R0 ← -(R3) ← LOC[new_scalar]
MOV ONE,(R0)+ ;assume true (1.0)
CLR (R0)
JSR PC,YESCMP ;OK to compact now
CFCC ;copy condition flags from compare
RTS PC ; & Return
SLT: JSR PC,COMP ;compare the args
BLT 1$ ;if true then done
CLR @(R3) ;else set answer to false (0)
1$: RTS PC ; & return
SLE: JSR PC,COMP ;compare the args
BLE 1$ ;if true then done
CLR @(R3) ;else set answer to false (0)
1$: RTS PC ; & return
SGT: JSR PC,COMP ;compare the args
BGT 1$ ;if true then done
CLR @(R3) ;else set answer to false (0)
1$: RTS PC ; & return
SGE: JSR PC,COMP ;compare the args
BGE 1$ ;if true then done
CLR @(R3) ;else set answer to false (0)
1$: RTS PC ; & return
SEQ: JSR PC,COMP ;compare the args
BEQ 1$ ;if true then done
CLR @(R3) ;else set answer to false (0)
1$: RTS PC ; & return
SNE: JSR PC,COMP ;compare the args
BNE 1$ ;if true then done
CLR @(R3) ;else set answer to false (0)
1$: RTS PC ; & return
AND: LDF @(R3)+,AC0 ;Get first arg
LDF @(R3)+,AC1 ;Get second arg (and set condition flags)
JSR PC,NOCMP ;Don't compact for a bit
JSR PC, GETSCA ;R0 ← -(R3) ← LOC[new_scalar]
CLR (R0)+ ;assume false (0)
CLR (R0)
JSR PC,YESCMP ;OK to compact now
CFCC ;copy condition flags for 2nd arg
BEQ 1$ ;if it's false return false
TSTF AC0 ;else look at 1st arg
CFCC
BEQ 1$
MOV ONE,@(R3) ;if both args are true return true (1.0)
1$: RTS PC ; Return
LOR: LDF @(R3)+,AC0 ;Get first arg
LDF @(R3)+,AC1 ;Get second arg (and set condition flags)
JSR PC,NOCMP ;Don't compact for a bit
JSR PC, GETSCA ;R0 ← -(R3) ← LOC[new_scalar]
MOV ONE,(R0)+ ;assume true (1.0)
CLR (R0)
JSR PC,YESCMP ;OK to compact now
CFCC ;copy condition flags from compare
BNE 1$ ;if it's true return true
TSTF AC0 ;else look at 1st arg
CFCC
BNE 1$
CLR @(R3) ;if both args are false return false (0)
1$: RTS PC ; Return
NOT: LDF @(R3)+,AC0 ;Get arg (and set condition flags)
JSR PC,NOCMP ;Don't compact for a bit
JSR PC, GETSCA ;R0 ← -(R3) ← LOC[new_scalar]
CLR (R0)+ ;assume false (0)
CLR (R0)
JSR PC,YESCMP ;OK to compact now
CFCC ;copy condition flags for arg
BNE 1$ ;if it's false return true
MOV ONE,@(R3) ; else return true
1$: RTS PC ; Return
;return scalars: SADD, SSUB, SMUL, SDIV, SNEG, VDOT, PVDOT, VMAG, SSBRTN
COMMENT ⊗ All timings are averages of 1000 runs. They take into
account the cost of the RTS but not the JSR. It is assumed that
GETSCA and GETVEC take no time. All routines on this page are
interpreter routines. ⊗
;30 microseconds
SADD: ;Scalar ← Scalar + Scalar
LDF @(R3)+,AC0 ;AC0 ← arg 2
ADDF @(R3)+,AC0 ;AC0 ← arg2 + arg1
JSR PC,GETSCA ;R0 ← -(R3) ← LOC[new scalar block]
STF AC0,@(R3) ;Store result
CCC ;Clear condition code.
RTS PC ;Done
SSUB: ;Scalar ← Scalar - Scalar
LDF @2(R3),AC0 ;AC0 ← arg 1
SUBF @(R3)+,AC0 ;AC0 ← arg1 - arg2
TST (R3)+ ;Move past first argument
JSR PC,GETSCA ;R0 ← -(R3) ← LOC[new scalar block]
STF AC0,@(R3) ;Store result
CCC ;Clear condition code.
RTS PC ;Done
;30 microseconds
SMUL: ;Scalar ← scalar * scalar
LDF @(R3)+,AC0 ;AC0 ← arg 2
MULF @(R3)+,AC0 ;AC0 ← arg2 * arg1
JSR PC,GETSCA ;R0 ← -(R3) ← LOC[new scalar block]
STF AC0,@(R3) ;Store result
CCC ;Clear condition code.
RTS PC ;Done
;33 microseconds
SDIV: ;Scalar ← Scalar / Scalar
LDF @(R3)+,AC1 ;AC1 ← arg 2
LDF @(R3)+,AC0 ;AC0 ← arg 1
DIVF AC1,AC0 ;AC0 ← arg1 / arg2
JSR PC,GETSCA ;R0 ← -(R3) ← LOC[new scalar block]
STF AC0,@(R3) ;Store result
CCC ;Clear condition code.
RTS PC ;Done
;26 microseconds
SNEG: ;Scalar ← -Scalar
LDF @(R3)+,AC0 ;AC0 ← arg
NEGF AC0 ;AC0 ← -arg
JSR PC,GETSCA ;R0 ← -(R3) ← LOC[new scalar block]
STF AC0,@(R3) ;Store result
CCC ;Clear condition code.
RTS PC ;Done
;96 -- 116 microseconds
VDOT: ;Scalar ← Vector dot Vector
;S ← (X1X2 + Y1Y2 + Z1Z2) / W1W2
MOV R2,-(SP) ;Save R2.
JSR PC,NOCMP ;Don't compact for a bit
MOV (R3)+,R1 ;R1 ← LOC[arg 2]
MOV (R3)+,R0 ;R0 ← LOC[arg 1]
CLRF AC0 ;AC0 ← 0. Running total
MOV #3,R2 ;R2 ← 3: Length of vector
1$: LDF (R0)+,AC1 ;Form sum of products of first 3 terms
MULF (R1)+,AC1 ;
ADDF AC1,AC0 ;
SOB R2,1$ ;Loop until all 3 fields done.
DIVF (R0),AC0 ;Divide by W1
DIVF (R1),AC0 ;Divide by W2. AC0 now has answer.
JSR PC,YESCMP ;OK to compact now
JSR PC,GETSCA ;R0 ← -(R3) ← LOC[new scalar block]
STF AC0,@(R3) ;Store result
MOV (SP)+,R2 ;Restore R2
CCC ;Clear condition code.
RTS PC ;Done
COMMENT ⊗
;103 -- 116 microseconds
PVDOT: ;Scalar ← Plane dot Vector
;S ← X1X2 + Y1Y2 + Z1Z2 + W1W2
MOV R2,-(SP) ;Save R2.
JSR PC,NOCMP ;Don't compact for a bit
MOV (R3)+,R1 ;R1 ← LOC[arg 2]
MOV (R3)+,R0 ;R0 ← LOC[arg 1]
CLRF AC0 ;AC0 ← 0. Running total
MOV #4,R2 ;R2 ← 4: Length of vector and weight
1$: LDF (R0)+,AC1 ;Form sum of products of all 4 terms
MULF (R1)+,AC1 ;
ADDF AC1,AC0 ;
SOB R2,1$ ;Loop until all 3 fields done.
JSR PC,YESCMP ;OK to compact now
JSR PC,GETSCA ;R0 ← -(R3) ← LOC[new scalar block]
STF AC0,@(R3) ;Store result
MOV (SP)+,R2 ;Restore R2
CCC ;Clear condition code.
RTS PC ;Done
⊗
;199 -- 207 microseconds
VMAGN: ;Scalar ← Norm (vector)
;S ← SQRT(XX + YY+ ZZ) / W
JSR PC,NOCMP ;Don't compact for a bit
MOV (R3)+,R1 ;R1 ← LOC[arg]
LDF (R1)+,AC0 ;AC0 ← X
MULF AC0,AC0 ;AC0 ← XX
LDF (R1)+,AC1 ;AC1 ← Y
MULF AC1,AC1 ;AC1 ← YY
ADDF AC1,AC0 ;AC0 ← XX + YY
LDF (R1)+,AC1 ;AC1 ← Z
MULF AC1,AC1 ;AC1 ← ZZ
ADDF AC1,AC0 ;AC0 ← XX + YY + ZZ
MOV R1,-(SP) ;Push LOC[W] onto system stack, to save across SQRTF
JSR PC,@LSQRTF ;AC0 ← SQRT(XX + YY + ZZ)
DIVF @(SP)+,AC0 ;AC0 ← AC0 / W
JSR PC,YESCMP ;OK to compact now
JSR PC,GETSCA ;R0 ← -(R3) ← LOC[new scalar block]
STF AC0,@(R3) ;Store answer
CCC ;Clear condition code.
RTS PC ;Done
SSBRTN: ;Call a routine.
LDF @(R3)+,AC0 ;AC0 ← arg
MOV @IPC(R4),R0 ;R0 ← which routine (a small number)
BMPIPC ;Bump IPC
ASL R0 ;Double (words → bytes)
BLE 1$ ;Too small.
CMP R0,#SBLSIZ ;Too large?
BGE 1$ ;Yes
JSR PC,@SBRLST(R0) ;Call a routine. AC0 ← answer.
JSR PC,GETSCA ;R0 ← -(R3) ← LOC[new scalar block]
STF AC0,@(R3) ;Store answer
CCC ;Clear condition code.
RTS PC ;Done
1$: ALERR SSBRMS ;Complain
SCC ;Set condition code
RTS PC ;Done
DATA
SSBRMS: ASCIE </NO SUCH SUBROUTINE/>
SBRLST: ;List of legal subroutines
0 ;Illegal
SQRT ;The only one right now. #1
SIN ;#2
COS ;#3
ASIN ;#4
ACOS ;#5
ATAN2 ;#6
SBLSIZ == .-SBRLST ;The size of the list (bytes)
CODE
SQRT: JMP @LSQRTF ;Let it do the returning
SIN: JMP @LSNCSD ;Let it do the returning
COS: JSR PC,@LSNCSD
LDF AC1,AC0
RTS PC
ASIN: JMP @LASIN ;Let it do the returning
ACOS: JMP @LACOS ;Let it do the returning
ATAN2: LDF @(R3)+,AC1 ;Get second argument for atan2(#1,#2)
JMP @LATAN2
;Vector utilities: UNITV, CROSV
;281 -- 286 microseconds
UNITV: ;Vector ← V / Norm(V)
;S ← SQRT(XX + YY+ ZZ)
JSR PC,NOCMP ;Don't compact for a bit
MOV (R3),R1 ;R1 ← LOC[arg]
LDF (R1)+,AC0 ;AC0 ← X
MULF AC0,AC0 ;AC0 ← XX
LDF (R1)+,AC1 ;AC1 ← Y
MULF AC1,AC1 ;AC1 ← YY
ADDF AC1,AC0 ;AC0 ← XX + YY
LDF (R1)+,AC1 ;AC1 ← Z
MULF AC1,AC1 ;AC1 ← ZZ
ADDF AC1,AC0 ;AC0 ← XX + YY + ZZ
JSR PC,@LSQRTF ;AC0 ← SQRT(XX + YY + ZZ)
JSR PC,GETVEC ;R0 ← -(R3) ← LOC[new vector block]
MOV 2(R3),R1 ;R1 ← LOC[old vector]
MOV #3,R2 ;R2 ← count of fields
1$: LDF (R1)+,AC1 ;AC1 ← field of vector
DIVF AC0,AC1 ;divide by norm
STF AC1,(R0)+ ;Store result
SOB R2,1$ ;Loop until done
MOV ONE,(R0)+ ;Set W to 1
CLR (R0) ; (two words long)
MOV (R3)+,(R3) ;Fix-up stack
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code
RTS PC ;Done
;172 -- 184 microseconds
CROSV: ;Vector ← Vector cross Vector
;X ← Y1Z2 - Y2Z1
;Y ← X2Z1 - X1Z2
;Z ← X1Y2 - X2Y1
;W ← W1W2
;AC0, 1, 2, 3, 4, 5 are garbaged by this routine.
JSR PC,NOCMP ;Don't compact for a bit
JSR PC,GETVEC ;R0 ← -(R3) ← LOC[new vector block]
MOV 2(R3),R2 ;R2 ← LOC[arg 2]
MOV 4(R3),R1 ;R1 ← LOC[arg 1]. Must not pop R3 stack yet!
LDF 14(R1),AC0 ;AC0 ← W1
MULF 14(R2),AC0 ;AC0 ← W1W2
STF AC0,14(R0) ;Store AC0 → W
LDF 4(R1),AC0 ;AC0 ← Y1
LDF (R2),AC1 ;AC1 ← X2
LDF 4(R2),AC2 ;AC2 ← Y2
LDF (R1),AC3 ;AC3 ← X1
STF AC3,AC4 ;AC4 ← X1
STF AC0,AC5 ;AC5 ← Y1
MULF AC2,AC3 ;AC3 ← X1Y2
MULF AC1,AC0 ;AC0 ← X2Y1
SUBF AC0,AC3 ;AC3 ← X1Y2 - X2Y1
STF AC3,10(R0) ;Z ← AC3
LDF 10(R2),AC0 ;AC0 ← Z2
LDF 10(R1),AC3 ;AC3 ← Z1
MULF AC4,AC0 ;AC0 ← X1Z2
MULF AC3,AC1 ;AC1 ← X2Z1
SUBF AC0,AC1 ;AC1 ← X2Z1 - X1Z2
STF AC1,4(R0) ;Y ← AC1
LDF 10(R2),AC0 ;AC0 ← Z2
MULF AC5,AC0 ;AC0 ← Y1Z2
MULF AC2,AC3 ;AC3 ← Y2Z1
SUBF AC3,AC0 ;AC0 ← Y1Z2 - Y2Z1
STF AC0,(R0) ;X ← AC0
MOV (R3)+,2(R3) ;Put result cell where first arg was
TST (R3)+ ; & fix-up stack
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code
RTS PC ;Done
;TRANS extraction routines: TPOS, TORIEN, TAXIS, TMAGN
TPOS: ;Extracts the position part of a TRANS (last column)
JSR PC,NOCMP ;Don't compact for a bit
JSR PC,GETVEC ;R0 ← -(R3) ← LOC[New Vector]
MOV 2(R3),R1 ;R1 ← LOC[TRANS]
ADD #44,R1 ;R1 ← LOC [last column of TRANS]
MOV #6,R2 ;Three 2-word components to move
1$: MOV (R1)+,(R0)+ ;Copy it
SOB R2,1$
MOV ONE,(R0)+ ;Stick in the scale factor
CLR (R0)
MOV (R3)+,(R3) ;Fix-up stack
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition codes
RTS PC ; & Return
TORIEN: ;Extracts the rotation part of a TRANS
JSR PC,NOCMP ;Don't compact for a bit
JSR PC,GETTRN ;R0 ← -(R3) ← LOC[New TRANS]
MOV 2(R3),R1 ;R1 ← LOC[TRANS]
MOV #22,R2 ;Three columns to do, three 2-word #'s/col
1$: MOV (R1)+,(R0)+ ;Copy the ROTN
SOB R2,1$
MOV #6,R2
2$: CLR (R0)+ ;Set up last column, three 0's
SOB R2,2$
MOV (R3)+,(R3) ;Fix-up stack
JSR PC,YESCMP ;OK to compact now
CCC ;Clear cond codes
RTS PC ; & Return
;TAXIS & TANGLE routines to extract the axis vector and angle of rotation
; given a rotation (trans);
;Define some constants
DATA
ONE: .FLT2 1.0
TWO: .FLT2 2.0
CTHIRD: .FLT2 0.576 ;Square root of 1/3
C1001: .FLT2 1.0001
C0001: .FLT2 0.0001
CODE
TAXIS: JSR PC,TAXAN ;Get vector components in AC3,AC4 & AC5
TST (R3)+ ;Fix stack
JSR PC,GETVEC ;Get a new vector to store results
STF AC3,(R0)+
LDF AC4,AC0
STF AC0,(R0)+ ;Store X,Y & Z components
LDF AC5,AC0
STF AC0,(R0)+
MOV ONE,(R0)+ ;Store scale factor of 1
CLR (R0)
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition codes
RTS PC ; & Return
TMAGN: JSR PC,TAXAN ;Get COS(angle) in AC0, vector components in AC 3-5
STF AC3,-(SP) ;Store X component
JSR PC,@LACOS ;Compute angle in AC0
LDF (SP)+,AC3 ;Retrieve X
LDF CTHIRD,AC1 ;Square root of 1/3
LDF AC3,AC2 ;Get X
ABSF AC2
CMPF AC2,AC1 ;ABS(X)-SQRT(1/3)
CFCC ;Copy FPP cond codes into CPU cond codes
BLT 1$
LDF 34(R2),AC1 ;Get (2,3)
SUBF 24(R2),AC1 ;(2,3) - (3,2)
MULF AC3,AC1 ;Get sign of SIN(angle)
BR 4$
1$: LDF AC4,AC2 ;Get Y
ABSF AC2
CMPF AC2,AC1 ;ABS(Y)-SQRT(1/3)
CFCC ;Copy FPP cond codes into CPU cond codes
BLT 2$
LDF 10(R2),AC1 ;Get (3,1)
SUBF 30(R2),AC1 ;(3,1) - (1,3)
MULF AC4,AC1 ;Get sign of SIN(angle)
BR 4$
2$: LDF AC5,AC2 ;Get Z
ABSF AC2
CMPF AC2,AC1 ;ABS(Z)-SQRT(1/3)
CFCC ;Copy FPP cond codes into CPU cond codes
BLT 3$
LDF 14(R2),AC1 ;Get (1,2)
SUBF 4(R2),AC1 ;(1,2) - (2,1)
MULF AC5,AC1 ;Get sign of SIN(angle)
BR 4$
3$: ALERR TMAGMS ;Complain
CLRF AC0 ;& return NILROT
4$: CFCC
BLT 5$
NEGF AC0 ;If SIN(angle) > 0 then negate angle
5$: TST (R3)+ ;Clean up stack
JSR PC,YESCMP ;OK to compact now
JSR PC,GETSCA ;Get a scalar
STF AC0,@(R3) ;Store the angle of rotation
CCC ;Clear condition codes
RTS PC ; & Return
DATA
TMAGMS: ASCIE </ROTATION STRANGENESS/>
CODE
TAXAN: ;Code common to both TAXIS & TMAGN
JSR PC,NOCMP ;Don't compact for a bit
MOV (R3),R2 ;R2 points to the ROT
LDF (R2),AC0 ;(1,1)
ADDF 20(R2),AC0 ;(2,2)
ADDF 40(R2),AC0 ;AC0 ← [(1,1)+(2,2)+(3,3)-1]/2 = COS(angle)
SUBF ONE,AC0
STF AC0,AC3 ;we'll use this later
DIVF TWO,AC0
STF AC0,AC1 ;Make a copy
ABSF AC1
CMPF C1001,AC1 ;If ABS(COS(angle)) > 1.0001 return the NILROT
CFCC
BGT 1$ ;Else go and compute the axis of rotation
CLRF AC0
STF AC0,AC3
STF AC0,AC4 ;NILROT = 0 degrees about (0,0,1)
LDF ONE,AC1
STF AC1,AC5
RTS PC
1$: STF AC0,-(SP) ;Store COS(angle) away for later
NEGF AC3
ADDF TWO,AC3 ;AC3 ← 3 - (1,1) - (2,2) - (3,3)
LDF ONE,AC0
SUBF (R2),AC0 ;(1,1)
SUBF 20(R2),AC0 ;(2,2)
ADDF 40(R2),AC0 ;(3,3)
DIVF AC3,AC0 ;AC0 ← Z↑2
CMPF C0001,AC0
CFCC
BLT 3$ ;If Z > 0.0001 skip ahead
CLRF AC5 ;Set Z ← 0
LDF ONE,AC0
SUBF (R2),AC0 ;(1,1)
ADDF 20(R2),AC0 ;(2,2)
SUBF 40(R2),AC0 ;(3,3)
DIVF AC3,AC0 ;AC0 ← Y↑2
CMPF C0001,AC0
CFCC
BLT 2$ ;If Y > 0.0001 skip ahead
CLRF AC4 ;Set Y ← 0
LDF ONE,AC3 ;Set X ← 1
BR 5$ ;Skip to end
2$: JSR PC,@LSQRTF ;Get SQRT(Y↑2)
STF AC0,AC4
LDF AC5,AC2 ;Clear this for later
BR 4$ ;Skip ahead to where X is computed
3$: JSR PC,@LSQRTF ;Get SQRT(Z↑2)
STF AC0,AC5
LDF ONE,AC2
STF AC2,AC3 ;For later
SUBF (R2),AC2 ;(1,1)
LDF 14(R2),AC0 ;(1,2)
DIVF AC2,AC0 ;AC0 ← (1,2) / [ 1 - (1,1) ]
LDF 10(R2),AC2 ;(3,1)
MULF AC0,AC2
ADDF 20(R2),AC2 ;(3,2)
MULF 4(R2),AC0 ;(2,1)
SUBF AC0,AC3
SUBF 20(R2),AC3 ;(2,2)
DIVF AC3,AC2 ;AC2 ← [(3,2)+(3,1)*(1,2)/[1-(1,1)] /
; [1-(2,2)-(2,1)*(1,2)/[1-(1,1)]
MULF AC5,AC2
STF AC2,AC4 ;AC4 ← Y
LDF 10(R2),AC2 ;(3,1)
MULF AC5,AC2 ;Z
4$: LDF 4(R2),AC3 ;(2,1)
MULF AC4,AC3 ;Y
ADDF AC2,AC3
LDF ONE,AC1
SUBF (R2),AC1 ;(1,1)
DIVF AC1,AC3 ;AC3 ← [(2,1)*Y+(3,1)*Z] / [1-(1,1)] = X
5$: LDF (SP)+,AC0 ;Retrieve the COS(angle)
RTS PC ; & Return to TAXIS or TMAGN
;Return vectors: SVMUL, TVMUL, VMAKE, VADD, VSUB
;83 -- 91 microseconds
SVMUL: ;Vector ← Scalar * Vector. Interpreter routine
;X ← S*X, Y ← S*Y, Z ← S*Z, W ← W
JSR PC,NOCMP ;Don't compact for a bit
JSR PC,GETVEC ;R0 ← -(R3) ← LOC[new vector block]
MOV 2(R3),R2 ;R2 ← LOC[arg2] (the vector)
LDF @4(R3),AC0 ;AC0 ← arg1 (the scalar)
MOV #3,R1 ;R1 ← 3: How many fields to handle
1$: LDF (R2)+,AC1 ;AC1 ← next field of vector
MULF AC0,AC1 ;AC1 ← product
STF AC1,(R0)+ ;Store result
SOB R1,1$ ;Loop until all 3 fields done.
MOV (R2)+,(R0)+ ;Transfer W
MOV (R2)+,(R0)+ ; which is 2 words long.
MOV (R3)+,2(R3) ;Fix-up stack
TST (R3)+
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code
RTS PC ;Done
VMAKE: ;Interpreter routine
LDF @(R3)+,AC3 ;Fetch arg3 (Z)
LDF @(R3)+,AC2 ;Fetch arg2 (Y)
LDF @(R3)+,AC1 ;Fetch arg1 (X)
JSR PC,NOCMP ;Don't compact for a bit
JSR PC,GETVEC ;R0 ← -(R3) ← LOC[new vector]
STF AC1,(R0)+ ;Store X
STF AC2,(R0)+ ;Store Y
STF AC3,(R0)+ ;Store Z
MOV ONE,(R0)+ ;Store W
CLR (R0) ;Store W (second word)
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code
RTS PC ;Done
VADD: ;Interpreter routine
JSR PC,NOCMP ;Don't compact for a bit
MOV (R3)+,R0 ;R0 ← LOC[arg 2] (a vector)
MOV (R3)+,R1 ;R1 ← LOC[arg 1] (a vector)
LDF (R0)+,AC1 ;Calculate X
ADDF (R1)+,AC1 ;
LDF (R0)+,AC2 ;Calculate Y
ADDF (R1)+,AC2 ;
LDF (R0)+,AC3 ;Calculate Z
ADDF (R1)+,AC3 ;
VRET: JSR PC,GETVEC ;R0 ← -(R3) ← LOC[new vector]
STF AC1,(R0)+ ;Store X
STF AC2,(R0)+ ;Store Y
STF AC3,(R0)+ ;Store Z
MOV ONE,(R0)+ ;Assume W is 1
CLR (R0) ;
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code
RTS PC ;Done
VSUB: ;Interpreter routine
JSR PC,NOCMP ;Don't compact for a bit
MOV (R3)+,R1 ;R1 ← LOC[arg 2] (a vector)
MOV (R3)+,R0 ;R0 ← LOC[arg 1] (a vector)
LDF (R0)+,AC1 ;Calculate X
SUBF (R1)+,AC1 ;
LDF (R0)+,AC2 ;Calculate Y
SUBF (R1)+,AC2 ;
LDF (R0)+,AC3 ;Calculate Z
SUBF (R1)+,AC3 ;
BR VRET ;Use common end code in VADD above
;283 -- 324 microseconds
TVMUL: ;Vector ← Trans * Vector. Interpreter routine
JSR PC,NOCMP ;Don't compact for a bit
MOV (R3),R2 ;R2 ← LOC[arg2] (the vector)
MOV 2(R3),R0 ;R0 ← LOC[arg1] (the trans)
CLRF AC1 ;X ← 0
CLRF AC2 ;Y ← 0
CLRF AC3 ;Z ← 0
MOV #4,R1 ;R1 ← How many columns left to go
1$: LDF (R2)+,AC0 ;AC0 ← field of vector
STF AC0,AC5 ;AC5 ← copy of AC0
MULF (R0)+,AC0 ;
ADDF AC0,AC1 ;Add partial result to X
LDF AC5,AC0 ;Restore AC0
MULF (R0)+,AC0 ;
ADDF AC0,AC2 ;Add partial result to Y
LDF AC5,AC0 ;Restore AC0
MULF (R0)+,AC0 ;
ADDF AC0,AC3 ;Add partial result to Z.
SOB R1,1$ ;Go back to do all 4 columns.
JSR PC,GETVEC ;R0 ← -(R3) ← LOC[new vector]
STF AC1,(R0)+ ;Store X
STF AC2,(R0)+ ;Store Y
STF AC3,(R0)+ ;Store Z
MOV -4(R2),(R0)+;Copy W from the vector
MOV -2(R2),(R0) ; (2 words long)
MOV (R3)+,2(R3) ;Put result cell where first arg was
TST (R3)+ ; & fix up stack
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code
RTS PC ;Done
;Return a trans: TMAKE, TVADD, TVSUB, TTMUL, TINVRT, VSAXWR
TMAKE: ;Interpreter routine.
;All that is required is to take the rot part of the first argument,
;and the vector from the second part;
JSR PC,NOCMP ;Don't compact for a bit
JSR PC,GETTRN ;R0 ← -(R3) ← LOC[new trans]
MOV 4(R3),R2 ;R2 ← LOC[arg 1] (the trans)
MOV #11,R1 ;R1 ← Count of how many copies to make
1$: MOV (R2)+,(R0)+ ;Transfer first half of floating word
MOV (R2)+,(R0)+ ;Transfer second half of floating word
SOB R1,1$ ;Repeat until done
MOV 2(R3),R2 ;R2 ← LOC[arg 2] (the vector)
MOV #3,R1 ;R1 ← Count of how many copies to make
2$: MOV (R2)+,(R0)+ ;Transfer first half of floating word
MOV (R2)+,(R0)+ ;Transfer second half of floating word
SOB R1,2$ ;Repeat until done
MOV (R3)+,2(R3) ;Fix-up stack
TST (R3)+
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code.
RTS PC ;Done.
TVCOM: ;Utility routine used to do common code in TVADD & TVSUB
JSR PC,NOCMP ;Don't compact for a bit
JSR PC,GETTRN ;R0 ← -(R3) ← LOC[new trans]
MOV 2(R3),R2 ;R2 ← LOC[arg 2] (the vector)
MOV 4(R3),R1 ;R1 ← LOC[arg 1] (the trans)
MOV #11,R3 ;R3 ← Count of how many copies to make
1$: MOV (R1)+,(R0)+ ;Transfer first half of floating word
MOV (R1)+,(R0)+ ;Transfer second half of floating word
SOB R3,1$ ;Repeat until done
MOV #3,R3 ;R3 ← Count of how many additions to perform
RTS PC ;Return to TVADD or TVSUB
TVADD: ;Interpreter routine.
;All that is required is to take the rot part of the first argument,
;and add the vector from the first part to the second argument.
MOV R3,-(SP) ;Save R3
JSR PC,TVCOM ;Do the common code for TVADD & TVSUB
1$: LDF (R1)+,AC0 ;AC0 ← word from trans
ADDF (R2)+,AC0 ; + word from vector
STF AC0,(R0)+ ;
SOB R3,1$ ;Repeat until done
TVRET: MOV (SP)+,R3 ;Restore R3
MOV -2(R3),2(R3) ;Fix-up stack (pretty strange huh?)
TST (R3)+
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code.
RTS PC ;Done.
TVSUB: ;Interpreter routine.
;All that is required is to take the rot part of the first argument,
;and subtract the second argument from the vector of the first arg.
MOV R3,-(SP) ;Save R3
JSR PC,TVCOM ;Do the common code for TVADD & TVSUB
1$: LDF (R1)+,AC0 ;AC0 ← word from trans
SUBF (R2)+,AC0 ; + word from vector
STF AC0,(R0)+ ;
SOB R3,1$ ;Repeat until done
BR TVRET ;Do common end code & return
TTMUL: ;Interpreter routine
;Multiplies two transes together.
MOV R4,-(SP) ;Save R4
JSR PC,NOCMP ;Don't compact for a bit
JSR PC,GETTRN ;R0 ← -(R3) ← LOC[new trans]
MOV 2(R3),R2 ;R2 ← LOC[arg 2]
MOV 4(R3),R4 ;R4 ← LOC[arg 1]
MOV R3,-(SP) ;Save R3
MOV R4,-(SP) ;Save a copy of R4
MOV #4,R1 ;Loop count for cols of answer
1$: LDF (R2)+,AC1 ;Pick up a column of arg2: First row
LDF (R2)+,AC2 ; Second row
LDF (R2)+,AC3 ; Third row
STF AC3,AC4 ; store in AC4
MOV #3,R3 ;Loop count for rows of answer
2$: LDF (R4),AC3 ;First col of arg 1
MULF AC1,AC3 ;
LDF 14(R4),AC0 ;Second col of arg 1
MULF AC2,AC0 ;
ADDF AC0,AC3 ;
LDF 30(R4),AC0 ;Third col of arg 1
MULF AC4,AC0 ;
ADDF AC0,AC3 ;
STF AC3,(R0)+ ;
ADD #4,R4 ;Move to the next column of arg 1
SOB R3,2$ ;Repeat for first 3 rows of answer
MOV (SP),R4 ;Reset R4 to point to first row of arg 1
SOB R1,1$ ;Repeat for all four columns of answer
LDF -14(R0),AC1 ;Add correction for last column, first row
ADDF 44(R4),AC1 ;
STF AC1,-14(R0) ;
LDF -10(R0),AC1 ;Add correction for last column, second row
ADDF 50(R4),AC1 ;
STF AC1,-10(R0) ;
LDF -4(R0),AC1 ;Add correction for last column, third row
ADDF 54(R4),AC1 ;
STF AC1,-4(R0) ;
TST (SP)+ ;Pop the R4 temp
MOV (SP)+,R3 ;Restore R3
MOV (SP)+,R4 ;Restore R4
MOV (R3)+,2(R3) ;Fix-up stack
TST (R3)+
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code
RTS PC ;Done
TINVRT: ;Interpreter routine
COMMENT ⊗ Inverts a trans.
The result, (rot',trslat'), is defined:
rot' = transpose(rot)
trslat' = -(rot'*trslat)
⊗
JSR PC,NOCMP ;Don't compact for a bit
JSR PC,GETTRN ;R0 ← -(R3) ← LOC[new trans] + 4*interation number
MOV 2(R3),R2 ;R2 ← LOC[old trans], travels down the whole trans
MOV R3,-(SP) ;Save R3
MOV R4,-(SP) ;Save R4
MOV R0,R3 ;R3 ← LOC[new trans] + 20*interation number
MOV R2,R4 ;R4 ← LOC[old trans], stays constant
MOV #3,R1 ;Three columns to do
1$: ;Transpose a column, multiplying by the translation
CLRF AC1 ;Cumulative product
LDF (R2)+,AC0 ;Take from the source rotation
STF AC0,(R0) ; into the transpose,
MULF 44(R4),AC0 ;
SUBF AC0,AC1 ;accumulate the product.
LDF (R2)+,AC0 ;Take from the source rotation
STF AC0,14(R0) ; into the transpose,
MULF 50(R4),AC0 ;
SUBF AC0,AC1 ;accumulate the product.
LDF (R2)+,AC0 ;Take from the source rotation
STF AC0,30(R0) ; into the transpose
MULF 54(R4),AC0 ;
SUBF AC0,AC1 ;accumulate the product
STF AC1,44(R0) ;Place the new translation
ADD #4,R0 ;Move to next row of result
ADD #14,R3 ;Move to next column of result
SOB R1,1$ ;
MOV (SP)+,R4 ;Restore R4
MOV (SP)+,R3 ;Restore R3
MOV (R3)+,(R3) ;Fix-up stack
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code
RTS PC ;Done
VSAXWR: ;Interpreter Routine coded by ARG 5/3/76
;Produces a trans that rotates about a vector by a given angle
MOV R5,-(SP) ;Save R5
LDF @(R3)+,AC2 ;Save angle in AC2
JSR PC,UNITV ;Convert vector to unit vector
STF AC2,AC0 ;Retrieve angle
JSR PC,@LSNCSD ;Get sin & cos of angle
STF AC0,AC4 ;Save sin in AC4
STF AC1,AC5 ;Save cos in AC5
SUBF ONE,AC1 ;AC1←(1-COS)
NEGF AC1
JSR PC,NOCMP ;Don't compact for a bit
JSR PC,GETTRN ;R0←-(R3)←LOC[New Tran]
MOV 2(R3),R1 ;R1←LOC[Unit Vec]
MOV #3,-(SP) ;Three columns to do
1$: MOV #3,R5 ;Three rows to do
MOV 2(R3),R2 ;R2←LOC[Unit vec]
LDF AC1,AC2
MULF (R1)+,AC2 ;AC2←(1-COS)*U[i]
2$: LDF AC2,AC3
MULF (R2)+,AC3 ;Trans[j,i]←(1-COS)*U[i]*U[j]
STF AC3,(R0)+
SOB R5,2$ ;Do all 3 rows
DEC (SP)
BGT 1$ ;Do all 3 columns
MOV (SP)+,(R0)+
CLR (R0)+ ;Set up last column
CLR (R0)+
CLR (R0)+
CLR (R0)+
CLR (R0)+
MOV #3,R5 ;Three terms to do: (1,1) (2,2) & (3,3)
MOV (R3),R0 ;R0←LOC[Trans]
3$: LDF AC5,AC1 ;AC1←COS
ADDF (R0),AC1 ;Add COS to (1-COS)*U[i]*U[i] term
STF AC1,(R0)
ADD #20,R0 ;R0 points to next term to add COS to
SOB R5,3$ ;Do all three terms
MOV (R3),R0 ;R0←LOC[Trans]
MOV 2(R3),R1 ;R1←LOC[Unit Vec]
LDF (R1)+,AC2 ;AC2←U[X]
MULF AC4,AC2 ;AC2←SIN*U[X]
STF AC2,AC3 ;Make a copy
ADDF 24(R0),AC2 ;Add it to the (3,2) term
STF AC2,24(R0)
NEGF AC3
ADDF 34(R0),AC3 ;Subtract it from the (2,3) term
STF AC3,34(R0)
LDF (R1)+,AC2 ;AC2←U[Y]
MULF AC4,AC2 ;AC2←SIN*U[Y]
STF AC2,AC3 ;Make a copy
ADDF 30(R0),AC2 ;Add it to the (1,3) term
STF AC2,30(R0)
NEGF AC3
ADDF 10(R0),AC3 ;Subtract it from the (3,1) term
STF AC3,10(R0)
LDF (R1)+,AC2 ;AC2←U[Z]
MULF AC4,AC2 ;AC2←SIN*U[Z]
STF AC2,AC3 ;Make a copy
ADDF 4(R0),AC2 ;Add it to the (2,1) term
STF AC2,4(R0)
NEGF AC3
ADDF 14(R0),AC3 ;Subtract it from the (1,2) term
STF AC3,14(R0) ;Trans is done!
MOV (SP)+,R5 ;Restore R5
MOV (R3)+,(R3) ;Clean up stack
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition codes
RTS PC ; & Return
;Motion: MOVE, CENTER
MOVE: ;Interpreter routine
MOV LMOVE,R2 ;Set for moving operation
JMP MOVSTA ;Use the common move code
CENTER: ;Interpreter routine
MOV LCENTER,R2 ;Set for centering operation
JMP MOVSTA ;Use the common move code
DVBKSZ == 12 ;Size of a device block
COMMENT ⊗ New version to update the frame afterwords. Assumes that
there are two arguments: a pointer to the trajectory table and a word
of mechanism bits, to help in updating the necessary variables. ⊗
MOVSTA: MOV #'π,R0 ;Whistle while you work
JSR PC,TYPCHR ;
MOV #DVBKSZ,R0 ;Get a device block
JSR PC,GTFREE ;
MOV R0,R1 ;R1 ← address of device block
MOV R0,-(SP) ;Save a copy on the stack
MOV @IPC(R4),R0 ;R0 ← address of coefficient list
JSR PC,NOCMP ;Don't compact for a bit
JSR PC,@R2 ;Do some kind of move (MOVE, CENTER)
JSR PC,YESCMP ;OK to compact now
TST R0 ;Did the move succeed?
BEQ 1$ ;Yes
MOV R0,-(SP) ; save error code
MOV #MVMES1,R0 ;
JSR PC,TYPSTR ; Complain
MOV (SP)+,R0 ;
BIT #177400,R0 ;Associated joint #?
BEQ 12$ ; No skip ahead
MOV (SP),R1 ;Get address of device block
MOV (R1)+,R2 ;Maximum number of joints in device block
TST (R1)+ ;Point to first joint
10$: BIT #177400,(R1)+ ;Is this the offending joint?
BNE 11$ ; Yup - found it
SOB R2,10$ ;Try next joint
BR 12$
11$: MOV -(R1),R0 ;Change R0 so the low 2 digits give joint #
12$: JSR PC,TYPOCT ; Give error condition
MOV (SP),R1 ; put address of device block in R1
ALERR MOVERR ; and switch to DDT
1$: BMPIPC ;Bump IPC
MOV @IPC(R4),R2 ;R2 ← mechanism bits
BMPIPC ;Bump IPC
;Invalidate the affected device variables;
.IFNZ YELLOW
BIT #YARM,R2 ;
BEQ 2$ ;
MOV #YACOFS,R0 ;
JSR PC,GETARG ;
MOV (R0),R0 ;
JSR PC,INVLDT ;
2$: BIT #YHAND,R2 ;
BEQ 3$ ;
MOV #YHCOFS,R0 ;
JSR PC,GETARG ;
MOV (R0),R0 ;
JSR PC,INVLDT ;
.ENDC
3$: BIT #BARM,R2 ;
BEQ 4$ ;
MOV #BACOFS,R0 ;
JSR PC,GETARG ;
MOV (R0),R0 ;
JSR PC,INVLDT ;
4$: BIT #BHAND,R2 ;
BEQ 5$ ;
MOV #BHCOFS,R0 ;
JSR PC,GETARG ;
MOV (R0),R0 ;
JSR PC,INVLDT ;
5$: MOV (SP)+,R0 ;
JSR PC,RLFREE ;Get rid of the device block
CCC ;Clear condition code
RTS PC ;Return
RETRY: TST (SP)+ ;Get here from ALERR; clean off stack
MOV (SP)+,R0 ;
JSR PC,RLFREE ;Get rid of the device block
BACKIPC ;Backup IPC
RTS PC ;
DATA
MVMES1: ASCIE </
SERVO ERROR = />
MOVERR: ASCIE </DEVICE BLOCK AT (R1). TO RETRY THE MOVE, RETRY$G/>
CODE
; STOP, TABOFS, WHERE, NOTICE
STOP: ;Interpreter routine
COMMENT ⊗ Takes one argument, a set of mechanism bits. (e.g. BARM,
BHAND, YARM, YHAND). For each one on, all the associated joints are
stopped. ⊗
MOV @IPC(R4),R2 ;R2 ← mechanism bits
BMPIPC ;Bump IPC
MOV R2,R0 ;R0 ← mech bits
JSR PC,TABOFS ;R0 ← table offset
BIT #AHAND,R2 ;A hand?
BNE 1$ ;Yes
MOV #6,R1 ;R1 ← count of joints
BR 2$
1$: MOV #1,R1 ;R1 ← count of joints
2$: ADD LDVCPTR,R0 ;R0 ← LOC[table of device pointers]
3$: MOV (R0)+,R2 ;R2 ← device block
BEQ 4$ ;If any
BIS #100000,@0(R2) ;Stop this device.
4$: SOB R1,3$ ;Repeat
CCC ;Clear condition code
RTS PC ;Done
COMMENT ⊗ Certain tables are available via COMTAB entries. LERRPTR
points to the table ERRPTR of 16 words, one for each servo, which
points at the current error torques. LTHPTR points at the table THPTR
of 16 words, one for each servo, which points at the current joint
angles. ⊗
TABOFS:
COMMENT ⊗ R0 = Mechanism bit. Returns table offset (in bytes) in R0.
For example, if the mechanism is BARM, the OFBARM is returned. ⊗
BIT #YARM,R0 ;Is it this mechanism?
BEQ 1$ ;No
MOV #OFYARM,R0 ;Yes. Load up proper offset
RTS PC ; and return.
1$: BIT #YHAND,R0 ;Is it this mechanism?
BEQ 2$ ;No
MOV #OFYHAND,R0 ;Yes. Load up proper offset
RTS PC ; and return.
2$: BIT #BARM,R0 ;Is it this mechanism?
BEQ 3$ ;No
MOV #OFBARM,R0 ;Yes. Load up proper offset
RTS PC ; and return.
3$: BIT #BHAND,R0 ;Is it this mechanism?
BEQ 4$ ;No
MOV #OFBHAND,R0 ;Yes. Load up proper offset
RTS PC ; and return.
4$: ALERR TABMES ;Illegal
CLR R0 ;
RTS PC ;
DATA
TABMES: ASCIE </ILLEGAL MECHANISM/>
CODE
WHERE: ;Interpreter routine
COMMENT ⊗ One argument: The mechanism bits. Puts value of that
mechanism on the stack. Only one mechanism at a time, please! ⊗
MOV @IPC(R4),R2 ;Mechanism bits
BMPIPC ;Bump IPC
JSR PC,NOCMP ;Don't compact for a bit
BIT #AHAND,R2 ;A hand?
BNE 1$ ;No.
JSR PC,GETTRN ;R0 ← -(R3) ← LOC[new trans]
BR 2$
1$: JSR PC,GETSCA ;R0 ← -(R3) ← LOC[new scalar]
2$: MOV LTHPTR,R1 ;
JSR PC,@LUPDATE ;
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code
RTS PC ;Done
NOTICE:
COMMENT ⊗ The arms may have been moved without our knowledge. A call
to this routine calls @LWHERE to find the real locations, and
invalidates all manipulator variables. This routine should be called
from WAITE, but not from MOVE or CENTER. It may be called from DDT,
since it saves all registers. ⊗
MOV R0,-(SP) ;Save R0
MOV R1,-(SP) ;Save R1
STF AC0,-(SP) ;Save AC0
MOV #DVBKSZ,R0 ;Get a device block
JSR PC,GTFREE ;
MOV R0,R1 ;R1 ← address of device block
MOV R0,-(SP) ;Save a copy on the stack
MOV #MVDCOF,R0 ;Pointer to coefficient list
JSR PC,@LWHERE ;Update the servo blocks
;ignore any failure return.
MOV (SP)+,R0 ;Reclaim device block
JSR PC,RLFREE ;
;Invalidate all manipulator variables
MOV #BHCOFS,R0 ;
JSR PC,GETARG ;
MOV (R0),R0 ;
JSR PC,INVLDT ;
MOV #BACOFS,R0 ;
JSR PC,GETARG ;
MOV (R0),R0 ;
JSR PC,INVLDT ;
.IFNZ YELLOW
MOV #YHCOFS,R0 ;
JSR PC,GETARG ;
MOV (R0),R0 ;
JSR PC,INVLDT ;
MOV #YACOFS,R0 ;
JSR PC,GETARG ;
MOV (R0),R0 ;
JSR PC,INVLDT ;
.ENDC
LDF (SP)+,AC0 ;Restore AC0
MOV (SP)+,R1 ;Restore R1
MOV (SP)+,R0 ;Restore R0
RTS PC ;
DATA
.IFNZ YELLOW
MVDCOF: YHANDSB + YARMSB + BHANDSB + BARMSB
0
.IFF
MVDCOF: BHANDSB + BARMSB
0
.ENDC
CODE
;Condition monitors: CMMAK
COMMENT ⊗ This is the third version of condition monitors: modified by arg 5/77
(here refered to as c-m's). Hardware-type c-m's will be ready soon.
The basic operations are Creation, Enabling, Disabling, Destruction.
Creation causes a c-m control block to be set up, and pointed to by
the c-m variable. This block has the following fields: ⊗
II == 0
XX CMTYPE ;Type of c-m: event,expression,duration,force or hardware
CMEVT == 0 ;Event type c-m
CMEXP == 1 ;Expression type c-m
CMDRA == 2 ;Duration type c-m
CMFRC == 3 ;Force sensing type c-m
CMHRD == 4 ;Hardware monitor type c-m
XX CMSTRT ;Starting address of c-m: duration, force & hardware
XX CMISB ;LOC[ISB] of the c-m
XX CMBITS ;Bits needed for: force & hardware c-m's
XX CMSTAT ;Status bits for the c-m
CMENB == 1 ;set => enabled
CMDES == 2 ;set => to be destroyed
CMRUN == 4 ;set => c-m is currently running
CMCBSZ == II/2 ;Length in words of a c-m control block.
II == 4 ;for event & expression c-m's
XX CMSEVT ;The event used to awaken the tester upon enabling
XX CMTEVT ;The event for which this c-m tests, if any
COMMENT ⊗ The various types of condition monitors are each handled
differently. Basically each c-m is an independent process which runs
in parallel with the process that creates it. Each c-m is an interpreter
and runs at priority 1 (exception: the checker part of an expression c-m
runs at priority 3). When a c-m is created by CMMAK, new PDB, ISB and
CMCB blocks are made. For duration, force and hardware c-m's nothing
further is done until they are enabled or destroyed. Enabling causes
the c-m checker part to be interpreted and to place the c-m body in the
appropriate queue, so it will be run if & when the condition being
checked for occurs. Disabling removes the c-m from the queue. Destroying
the c-m causes it to be disabled and then it's PDB, ISB & CMCB are all
reclaimed. At the conclusion of the body if the c-m has been re-enabled
it reschedules itself in the appropriate queue and then dismisses.
Event and expression c-m's, after initialization, wait for the
gronking event CMSEVT. Enabling signals the event CMSEVT and sets
the enabled bit in CMSTAT. Disabling resets the enabled bit, and the
c-m will wait on the CMSEVT for future action. As long as the c-m is
enabled, it periodically wakes up, checks its status bits. If the
enable bit is reset, the c-m waits for CMSEVT. Else it checks the
condition. If it is satisfied, the c-m disables itself and
proceeds to the conclusion (at level 1, the conclusion should reset
itself to level 0 after all critical activity has been accomplished,
although this is not currently done.) Otherwise, it reschedules itself.
If the destroy bit should ever be set in CMSTAT, then the c-m will
destroy the event CMSEVT. Then it will reclaim the c-m control blocked
and will dismiss, never to return. (The pointer to the c-m in the
environment should be zeroed by the destroying angel.). ⊗
CMMAK: ;Interpreter routine
COMMENT ⊗ Takes three arguments, the offset of the nascent c-m, the
level-offset of the event that this monitor is to wait on, if any,
and the IPC of the c-m code. ⊗
MOV @IPC(R4),R2 ;R2 ← offset
BMPIPC ;Bump IPC
ADD ENV(R4),R2 ;R2 ← Pointer into environment
TST (R2) ;Already something there?
BEQ 1$
ALERR CMMMSG ;Yes. complain.
;Make a c-m control block
1$: MOV #CMCBSZ,R0
JSR PC,GTFREE ;R0 ← LOC[c-m control block]
MOV R0,(R2) ;Stuff into environment
MOV @IPC(R4),CMTYPE(R0) ;Get type of c-m
BMPIPC ;Bump IPC
MOV R0,-(SP) ;Save LOC[c-m control block]
;Prepare the c-m job
MOV @IPC(R4),CMSTRT(R0) ;Store away IPC of start of c-m code
BMPIPC ;Bump IPC
MOV CMSTRT(R0),R0 ;R0 ← IPC of c-m code
CLR R1 ;C-m's do not expire with events
JSR PC,SPAWN ;R0 ← process control block for c-m
MOV (SP),R1 ;R1 ← LOC[CMCB]
MOV PDBR4(R0),R2 ;R2 ← PR4 = LOC[c-m's interpeter status block]
MOV R2,CMISB(R1) ;Store away location of c-m's ISB
MOV R1,CMCB(R2) ;Stuff CMCB of the c-m
MOV #UGRSAV+UFPUSE+2,PDBSTA(R0) ;c-m's run with priority = 1
MOV #144040,UPSW(R0)
CMP #CMEXP,CMTYPE(R1) ;If expression c-m runs with priority = 3
BNE 2$
MOV #UGRSAV+UFPUSE+6,PDBSTA(R0) ;Change priority to 3
MOV #144140,UPSW(R0)
2$: MOV R0,R2 ;R2 ← new process descriptor block
;Set up the new environment
JSR PC,NEWENV ;R0 ← LOC[new environment]
MOV ENV(R4),SLINK(R0) ;Not necessary to set up OLEV, etc.
MOV PDBR4(R2),R1;
MOV R0,ENV(R1) ;
.IFNZ SMALLB
EVSIG SBEVT ;End of critical section - value stored
.ENDC
INC LEV(R1) ;
MOV (SP)+,R0 ;R0 ← LOC[CMCB]
CMP #CMEXP,CMTYPE(R0) ;See what type of c-m we've got
BLT 4$ ;Duration, force sensing or hardware - jump ahead
BEQ 3$ ;Expression c-m - skip over event c-m stuff
MOV R0,-(SP) ;Save LOC[c-m control block]
MOV @IPC(R4),R0 ;R0 ← level-offset of event this c-m waits for.
BMPIPC ;Bump IPC
JSR PC,GETARG
MOV R0,R1 ;R1 ← LOC[environment location of event]
MOV (SP)+,R0 ;R0 ← LOC[c-m control block]
MOV (R1),CMTEVT(R0) ;Put the CMTEVT in the c-m control block.
3$: EVMAK ;
MOV (SP)+,CMSEVT(R0) ;Make an event for CMSEVT
FORK R2,#INTERP,#USRDM ;Cause the c-m to be started. It will go into wait.
BR 5$ ;Done
4$: CMP #CMDRA,CMTYPE(R0)
BEQ 5$ ;If duration type then done
MOV @IPC(R4),CMBITS(R0) ;Get force sensing bits for c-m
BMPIPC ;Bump IPC
5$: CCC ;Clear condition code
RTS PC ;Done
DATA
CMMMSG: ASCIE </CMMAK: WILL CREATE EXISTENT CONDITION MONITOR/>
; CMENBL, CMDSBL, CMDEST, CMTRIG, CMSKED, CMUNCR
CMNEMS: ASCIE </TRYING TO TREAT NON-EXISTENT EVENT/>
CODE
CMENBL: ;Interpeter routine
; One argument, a level-offset pair for the c-m to enable.
MOV @IPC(R4),R0 ;R0 ← level-offset
BMPIPC ;Bump IPC
JSR PC,GETARG ;R0 ← pointer into environment
MOV (R0),R0 ;R0 ← pointer to c-m control block.
BEQ CMDERR ;If none, then error
CMP #CMDRA,CMTYPE(R0) ;see what type of c-m we've got
BGT 1$ ;If event or expression then skip ahead
BIT #CMENB,CMSTAT(R0) ;Already enabled?
BNE 2$ ;Then done
BIS #CMENB,CMSTAT(R0) ;Set enabled bit
BIT #CMRUN,CMSTAT(R0) ;See if currently running
BNE 2$ ; & if so we're done - it'll re-enable itself
MOV R4,-(SP) ;Save old ISB
MOV CMISB(R0),R4 ;Get new ISB
MOV CMSTRT(R0),IPC(R4) ;Set IPC to LOC[c-m checker]
MOV RF,-(SP) ;Save RF
MOV SP,RF ;RF ← LOC[Stack]
JSR PC,INTERP ;Go do it - CMDUR, CMFORCE & CMSENSE return
MOV (SP)+,R4 ;Restore old ISB
BR 2$ ;Done
1$: BIS #CMENB,CMSTAT(R0) ;Set the enable bit
EVSIG CMSEVT(R0) ;Gronk the c-m
2$: CCC ;Clear condition code
RTS PC ;Done
CMDSBL: ;Interpreter routine
; One argument, a level-offset pair for the c-m to disable.
MOV @IPC(R4),R0 ;R0 ← level-offset
BMPIPC ;Bump IPC
JSR PC,GETARG ;R0 ← pointer into environment
MOV (R0),R0 ;R0 ← pointer to c-m control block.
BEQ CMDERR ;If none, then error
JSR PC,CMDIS ;Go disable the c-m
CCC ;Clear condition code
RTS PC ;Done
CMDERR: ALERR CMNEMS ;
SCC ;Set condition code
RTS PC ;
CMDEST: ;Interpreter routine
COMMENT ⊗ Argument list. Each is an offset for the c-m to destroy.
The list is terminated with a zero entry. ⊗
MOV @IPC(R4),R1 ;R1 ← offset
BEQ 4$ ;If 0, then done
BMPIPC ;Bump IPC
ADD ENV(R4),R1 ;R1 ← pointer into environment
MOV (R1),R0 ;R0 ← LOC[c-m control block]
BEQ CMDERR ;If none, then error
BIS #CMDES,CMSTAT(R0) ;Set the destroy bit
CMP #CMDRA,CMTYPE(R0) ;See what type of c-m
BGT 2$ ;If event or expression c-m then handle below
BIT #CMRUN,CMSTAT(R0) ;If running it will destroy itself
BNE 3$ ; so we're done
MOV R1,-(SP) ;Save R1
JSR PC,CMDIS ;Make sure c-m's disabled
MOV CMISB(R0),R2 ;R2 ← LOC[c-m's ISB]
JSR PC,RLFREE ;Reclaim the c-m control block
MOV STKBAS(R2),R0 ;Reclaim interpreter stack
JSR PC,RLFREE
MOV PDB(R2),R0 ;Reclaim Process Descriptor Block
JSR PC,RLFREE
EVWAIT INTEVT ;Enter critical region.
MOV #ISTBLK,R0 ;The following unlinks this interpreter from the chain.
1$: MOV R0,R1
MOV NXTINT(R1),R0
CMP R0,R2 ;Have we found ours yet?
BNE 1$
MOV NXTINT(R2),NXTINT(R1) ; Yes. rechain.
EVSIG INTEVT ;Leave critical region.
MOV R2,R0 ;Reclaim Interpreter Status Block
JSR PC,RLFREE
MOV (SP)+,R1 ;Retrieve R1
BR 3$
2$: EVKIL CMSEVT(R0);Destroy the event. That ought to wake him up!
3$: CLR (R1) ;Remove c-m from environment
BR CMDEST ;Go do the next one.
4$: BMPIPC ;Bump IPC the last time
CCC ;Clear condition code
RTS PC ;Done
CMDIS:
COMMENT ⊗ Routine to disable a c-m, R0 ← LOC[CMCB] ⊗
BIT #CMENB,CMSTAT(R0) ;Check if currently enabled
BEQ 3$ ; if not - done
CMP #CMDRA,CMTYPE(R0) ;See what type of c-m
BGT 2$ ;Event & expression c-m's are easy - skip ahead
BEQ 2$ ;Can't do anything with duration c-m's now
CMP #CMFRC,CMTYPE(R0)
BLT 2$ ; ditto with hardware c-m's
MOV R0,-(SP) ;Save R0
MOV CMISB(R0),R1 ;R1 ← LOC[c-m's ISB]
MOV PDB(R1),R1 ;R1 ← LOC[c-m's PDB]
MOV CMBITS(R0),R0 ;R0 ← c-m's force sensing bits
JSR PC,@LFRCOFF ;Remove c-m from force signal list
TST R0
BEQ 1$
ALERR CMNODS ;Complain if error
1$: MOV (SP)+,R0 ;Restore R0
2$: BIC #CMENB,CMSTAT(R0) ;Clear the enable bit
3$: RTS PC ;Done
DATA
CMNODS: ASCIE </COULDN'T DISABLE FORCE CMON/>
CODE
CMTRIG: ;Interpeter routine
COMMENT ⊗ Should be executed only from a c-m. Sets the priority to 1
and disables the checker. ⊗
MOV CMCB(R4),R0 ;
1$: EVTST CMSEVT(R0);Eat all signals enabling the checker.
BCC 1$
BIC #CMENB,CMSTAT(R0) ;Clear the enable bit
SETPRI #1 ;Set the priority to 1
TST (SP)+ ;Discard old priority
CCC ;Clear condition code
RTS PC ;Done
CMSKED: ;Interpreter routine
COMMENT ⊗ Goes to sleep a while (currently, 100 milliseconds). Upon
awakening, checks the status bits of this checker, and either
dismisses, waits, or returns. ⊗
MOV CMCB(R4),R0 ;R0 ← c-m control block
CMP #CMEXP,CMTYPE(R0) ;See what type of c-m
BNE 1$ ;If event c-m skip ahead
SETPRI #3 ;In case the conclusion left it at 1 or 0.
TST (SP)+ ;Flush old priority
MOV @IPC(R4),-(SP) ;Waiting interval
BMPIPC ;Bump IPC
SLEEP ;Sleep a while
1$: BIT #CMDES,CMSTAT(R0) ;Destroy bit set?
BEQ 3$ ;No
EVKIL CMSEVT(R0);Yes. Kill the triggering event.
2$: JSR PC,RLFREE ;Return the c-m control block
JMP TERMINATE ;Use the interpeter terminate routine.
3$: BIT #CMENB,CMSTAT(R0) ;Enable bit set?
BNE 4$ ;Yes.
EVWAIT CMSEVT(R0);No. Wait until signaled by the enabler
BCS 2$ ;If the enabling event died, so must we.
BR 1$ ;Else start from the awakening point.
4$: MOV CMTEVT(R0),R1 ;R1 ← event to test for
BEQ 5$ ;If any
EVWAIT R1 ;Wait for event to happen
BCS 2$ ;If the signaling event died, so must we.
BIT #CMENB,CMSTAT(R0) ;Still enabled?
BNE 5$ ;Yes. May exit.
EVSIG R1 ;Oops, we were disabled! Resignal the event.
BR 1$ ;And try again.
5$: CCC ;Clear condition code
RTS PC ;Done
CMUNCR: ;Interpreter routine.
COMMENT ⊗ Used in body of c-m. Starts uncritical section. ⊗
SETPRI #1 ;Set the priority to 1
TST (SP)+ ;Flush old priority
CCC ;Clear condition code
RTS PC ;Done
; CMDONE, CMDUR, CMFORCE, CMSENSE, COMPLY, CMPOFF, VMKFRC, TFRCST
CMDONE: ;Interpreter routine
COMMENT ⊗ Ends duration, force sensing & hardware monitor c-m's. Checks if
c-m was re-enabled while running and if so it will interpret the c-m's checker
(and so doing the c-m will be re-queued). Then it dismisses. ⊗
MOV CMCB(R4),R0 ;Get c-m control block
BIC #CMRUN,CMSTAT(R0) ;Clear run bit
BIT #CMDEST,CMSTAT(R0) ;Destroy ourself
BEQ 1$
JSR PC,RLFREE ;Yup - reclaim CMCB
JMP TERMINATE ;Use interpreter terminate routine
1$: BIT #CMENB,CMSTAT(R0) ;See if we were re-enabled
BEQ 2$ ;Nope - go away
MOV CMSTRT(R0),IPC(R4) ;Reset IPC to LOC[c-m's checker]
MOV RF,-(SP) ;Save RF
MOV SP,RF ;RF ← LOC[Stack]
JSR PC,INTERP ;Re-queue it
2$: MOV PDB(R4),R0 ;R0 ← LOC[c-m's PDB]
MOV R3,PDBR3(R0) ;Make sure stack is okay
MOV PDBPC(R0),PDBR2(R0) ;Save new PC(if any) in R2 since DISMIS kills it
DISMIS ;Bye-bye
JMP (R2) ;If return here use R2 to get where we should be
CMDUR: ;Interpreter routine
COMMENT ⊗ Schedules c-m body to be executed in time seconds. (The time is
on the stack.) Then returns control using RF. ⊗
LDF @(R3)+,AC0 ;Get time to wait in seconds
MULF THOUS,AC0 ;Convert it to milliseconds
STCFI AC0,R0 ; & make it integer
SCHEDU PDB(R4),#1$,#USRDM,R0 ;Schedule the c-m body to start later
MOV RF,SP ;Restore stack
MOV -2(SP),RF ;RF ← old PC
RTS RF ;Just return
1$: MOV CMCB(R4),R0 ;R0 ← LOC[c-m's control block]
BIT #CMENB,CMSTAT(R0) ;See if we're still enabled
BNE 2$
DISMIS ;If not then go away
2$: JMP CMGO ;Set flags & go interpret the c-m's body
VMKFRC: ;Interpreter routine
COMMENT ⊗ Takes force vector (on R3 stack) and makes it into a frame with the x-axis
along the force vector. Always followed by a call to TFRCST which calls SETC. ⊗
JSR PC,UNITV ;Make it a unit vector
JSR PC,NOCMP ;Don't compact for a while
MOV (R3)+,R0 ;R0 ← LOC[unit vector]
LDF (R0)+,AC0 ;Get X
LDF (R0)+,AC1 ;Get Y
LDF (R0)+,AC2 ;Get Z
STF AC0,AC4 ;Copy X
STF AC1,AC5 ;Copy Y
JSR PC,GETTRN ;R0 ← -(R3) ← LOC[new trans]
STF AC0,(R0)+
STF AC1,(R0)+ ;Fill in 1st column with unit vector
STF AC2,(R0)+
MULF AC0,AC0 ;X↑2
MULF AC1,AC1 ;Y↑2
ADDF AC1,AC0 ;X↑2 + Y↑2
CFCC ;Check if X = Y = 0
BNE 1$ ; & if not skip ahead
CMP (R0)+,(R0)+
STF AC2,(R0)+ ;Next column is (0 Z 0)
NEGF AC2
CMP (R0)+,(R0)+
STF AC2,(R0) ;Last column is (-Z 0 0)
BR 2$ ;Jump ahead
1$: JSR PC,@LSQRTF ;get SQRT(X↑2 + Y↑2)
STF AC0,AC1 ;Copy SQRT(X↑2 + Y↑2)
NEGF AC5 ;Negate Y
DIVF AC5,AC0 ;a = -Y / SQRT(X↑2 + Y↑2)
DIVF AC4,AC1 ;b = X / SQRT(X↑2 + Y↑2)
STF AC0,(R0)+
STF AC1,(R0)+ ;Fill in 2nd column with (a b 0)
CMP (R0)+,(R0)+
STF AC2,AC3 ;Copy Z
MULF AC0,AC2 ;aZ
MULF AC1,AC3 ;bZ
NEGF AC3 ;-bZ
MULF AC4,AC1 ; bX
MULF AC5,AC0 ;-aY
ADDF AC0,AC1 ;bX - aY
STF AC3,(R0)+
STF AC2,(R0)+ ;Fill in 3rd column with(-bZ,aZ,bX-aY)
STF AC1,(R0)+ ; it's the cross product of the other 2 columns
2$: JSR PC,YESCMP ;OK to compact again
CCC
RTS PC ;Done - return
TFRCST: ;Interpreter routine
COMMENT ⊗ Gets force frame off of the R3 stack, arm & co-ordinate system bits follow
via the IPC. Calls SETC. ⊗
MOV @IPC(R4),R0 ;Get bits for SETC: arm & c-oord system (hand/table)
BMPIPC
MOV (R3)+,R1 ;R1 ← LOC[force coordinate matrix]
JSR PC,@LSETC ;Initialize the force system
TST R0
BEQ 1$
ALERR CMNSET ;Complain if any problems
1$: CCC
RTS PC ;Done - return
CMFORCE: ;Interpreter routine
COMMENT ⊗ Gets force value (scalar on R3 stack) and queues c-m on force signal list.
Then returns control using RF. ⊗
LDF @(R3)+,AC0 ;Get the force threshold value
MOV PDB(R4),R1 ;R1 ← LOC[c-m's PDB]
MOV CMCB(R4),R2 ;R2 ← LOC[c-m's control block]
MOV CMBITS(R2),R0 ;R0 ← c-m's force bits
MOV #CMGO,R2 ;R2 ← when triggered start below
JSR PC,@LFRCSIG ;Put the c-m in the force signal list
TST R0
BEQ 1$
ALERR CMNFRC ;Complain if any problems
1$: MOV RF,SP ;Restore stack
MOV -2(SP),RF ;RF ← old PC
RTS RF ;Just return
CMGO: MOV CMCB(R4),R0 ;R0 ← LOC[c-m's control block]
BIS #CMRUN,CMSTAT(R0) ;Set the run bit
BIC #CMENB,CMSTAT(R0) ;Clear the enable bit
MOV PDB(R4),R0 ;R0 ← LOC[c-m's PDB]
MOV USKMAX(R0),SP ;Reset stack pointer
JMP INTERP ;Go interpret the c-m's body
COMPLY: ;Interpreter routine
COMMENT ⊗ Gets magnitude of force to apply (scalar on R3 stack) and the control bits via
(the arm and force component to apply) follow via the IPC. ⊗
MOV @IPC(R4),R0 ;Get bits for COMPLY
BMPIPC
LDF @(R3)+,AC0 ;Get the force value
JSR PC,@LCOMPLY ;Set up the force to apply
TST R0
BEQ 1$
ALERR CMNCMP ;Complain if any problems
1$: CCC
RTS PC ;Done - return
CMPOFF: ;Interpreter routine
ALERR NOCMPF ;Complain - CMPOFF hasn't been written yet
CCC
RTS PC
CMSENSE: ALERR CMNOSE ;Aren't any of these guys yet
MOV RF,SP ;Restore stack
MOV -2(SP),RF ;RF ← old PC
RTS RF ;Just return
DATA
CMNSET: ASCIE </COULDN'T INITIALIZE FORCE SYSTEM/>
CMNFRC: ASCIE </COULDN'T QUEUE FORCE CMON/>
CMNCMP: ASCIE </COULDN'T SET UP FORCE COMPLIANCE/>
NOCMPF: ASCIE </CAN'T TURN OFF COMPLIANCE YET/>
CMNOSE: ASCIE </HARDWARE MONITORING ISN'T READY YET/>
CODE
;Events: MAKEVT, SIGNAL, WAITE, DESEVT, PAUSE
COMMENT ⊗ Events can be created (at the beginnings of blocks is the
usual place), signaled, awaited (in the middle of a block) and
destroyed (at the end of a block). Each event is a variable, that
is, it is refered to by a level-offset pair. However, its place in
the environment does not point to a graph node, since there is no
such thing as attachment to an event. The event itself is stored in
the environment. The garbage collector marking phase had better
understand this. ⊗
MAKEVT: ;Interpreter routine
COMMENT ⊗ A list of arguments, each of which is an offset. This list
is terminated by a zero entry. For each argument, a fresh event is
created and placed in the environment at the desired offset, current
level. ⊗
MOV @IPC(R4),R0 ;R0 ← offset
BIC #177400,R0 ;Get rid of level info.
BEQ 1$ ;If none, done
BMPIPC ;Bump IPC
ADD ENV(R4),R0 ;R0 ← pointer into environment
EVMAK ;Make an event.
MOV (SP)+,(R0) ;Stuff it away.
BR MAKEVT ;Repeat
1$: BMPIPC ;Bump IPC
CCC ;Clear condition code.
RTS PC ;Done
SIGNAL: ;Interpreter routine. Signal the event of the level-offset pair.
MOV @IPC(R4),R0 ;R0 ← level-offset pair.
BMPIPC ;Bump IPC
JSR PC,GETARG ;R0 ← equivalent pointer into environment
EVSIG (R0) ;Signal that event.
CCC ;Clear condition code.
RTS PC ;Done
WAITE: ;Interpreter routine. Wait on the event of the level-offset pair.
MOV @IPC(R4),R0 ;R0 ← level-offset pair.
BMPIPC ;Bump IPC
JSR PC,GETARG ;R0 ← equivalent pointer into environment
EVWAIT (R0) ;Wait on that event.
BCC 1$ ;Return OK?
JMP TERMINATE ;The event was destroyed. I guess we should depart cleanly.
1$: JSR PC,NOTICE ;Assume the world has gone awry.
CCC ;Clear condition code.
RTS PC ;Done
DESEVT: ;Interpreter routine
COMMENT ⊗ A list of arguments, each of which is an offset. This list
is terminated by a zero entry. For each argument, the event is
destroyed. ⊗
MOV @IPC(R4),R0 ;Get offset
BIC #177400,R0 ;Remove level info.
BEQ 1$ ;If none, done
BMPIPC ;Bump IPC
ADD ENV(R4),R0 ;R0 ← pointer into environment
EVKIL (R0) ;Kill the event
CLR (R0) ;Remove the event from the environment
BR DESEVT ;Repeat
1$: BMPIPC ;Bump IPC
CCC ;Clear condition code.
RTS PC ;Done
PAUSE: ;Interpreter routine
COMMENT ⊗ Pause n seconds, where n is on the stack. ⊗
LDF @(R3)+,AC0 ;AC0 ← wait time
MULF THOUS,AC0 ;AC0 ← time, in milliseconds
STCFI AC0,R0 ;R0 ← time in milliseconds
SLEEP R0 ;The pause that refreshes
CCC ;Clear Condition code
RTS PC ;Done
DATA
THOUS: .FLT2 1000.0
CODE
;Output routines: PRINT, VALPRN, VARPRN, TACKVAL, TYPVAL, CVFX
PRINT: ;Interpreter routine
MOV @IPC(R4),R0 ;R0 ← Address of string
BMPIPC ;Bump IPC
EVWAIT CSLEVT ;
JSR PC,TYPSTR ;Type it out
EVSIG CSLEVT ;
CCC ;Clear condition code
RTS PC ;Done
VARPRN:
COMMENT ⊗ Interpreter routine. Prints the graph node pointed to by
the level-offset of the argument. ⊗
JSR PC,GTVAL ;Let GTVAL put value on stack
JMP VALPRN ;And let VALPRN take it from there.
VALPRN:
COMMENT ⊗ Interpreter routine. Prints the value the top of the stack
and pops it. ⊗
MOV (R3)+,R0 ;R0 ← LOC[value cell]
JSR PC,TYPVAL ;Go print it.
CCC ;Clear condition codes
RTS PC ;And return
.IFNZ ALAID
TACKVAL:
COMMENT ⊗ R1 points to a value cell. R0 points to a string where the
value is to be placed. Places it there just as TYPVAL prints it out,
using common code. ⊗
MOV R2,-(SP) ;Save R2
MOV R3,-(SP) ;Save R3
MOV #TACKV,-(SP);Stack the address of the placing routine.
MOV R0,R2 ;Exchange R0, R1
MOV R1,R0 ;
MOV R2,R1 ;Now R0 = value cell, R1 = string where to put it.
JSR PC,TYPVL ;And do just as TYPVAL does.
MOV R1,R0 ;Put back the final string pointer
BR TYPVRT ;Return
TACKV:
COMMENT ⊗ R1 = string pointer, R0 = new addition. Use TACK to put it
on. ⊗
MOV R2,-(SP) ;Save R2
MOV R0,R2 ;
MOV R1,R0 ;
MOV R2,R1 ;Now R0 = string poiter, R1 = new addition.
JSR PC,TACK ;
MOV R0,R1 ;R1 = final string pointer.
MOV (SP)+,R2 ;Restore R2
RTS PC ;Done
.ENDC
TYPVAL:
COMMENT ⊗ R0 points to a value cell. Prints it according to its
type. Requires the floating package. ⊗
MOV R2,-(SP) ;Save R2
MOV R3,-(SP) ;Save R3
MOV #TYPSTR,-(SP) ;Stack the address of the typing routine.
EVWAIT CSLEVT ;
JSR PC,TYPVL ;
EVSIG CSLEVT ;
TYPVRT: TST (SP)+ ;Get rid of the address of typing routine.
MOV (SP)+,R3 ;Restore R3
MOV (SP)+,R2 ;Restore R2
RTS PC ;
;R0 = LOC[value cell], R1 = LOC[string] in some cases.
;R2, R3 are available for use.
TYPVL: MOV R0,R2 ;R2 ← LOC[value cell]
MOV #CRLFX,R0 ;CRLF
JSR PC,@2(SP) ;
CMPB #SCLID,TAGID(R2) ;A scalar?
BEQ 1$
CMPB #VCTID,TAGID(R2) ;A vector?
BEQ 4$
CMPB #TRNID,TAGID(R2) ;A trans?
BEQ 5$
1$: MOV #SNAME,R0 ;
JSR PC,@2(SP) ;"SCALAR "
MOV #OUTBUF,R0 ;
2$: LDF (R2),AC0 ;
JSR PC,CVFX ;
MOV #OUTBUF,R0 ;
JSR PC,@2(SP) ;
3$: MOV #CRLFX,R0 ;CRLF
JSR PC,@2(SP) ;
RTS PC ;Done
4$: MOV #VNAME,R0 ;
JSR PC,@2(SP) ;"VECTOR "
MOV #OUTBUF,R0 ;
LDF (R2)+,AC0 ;
JSR PC,CVFX ;
LDF (R2)+,AC0 ;
JSR PC,CVFX ;
BR 2$ ;Bum code for last field.
5$: MOV #TNAME,R0 ;
JSR PC,@2(SP) ;"TRANS "
MOV R3,-(SP) ;Save R3
MOV #3,R3 ;R3 ← Number of rows
6$: MOV #CRLFX,R0 ;
JSR PC,@4(SP) ;
MOV #OUTBUF,R0 ;
LDF (R2),AC0 ;
JSR PC,CVFX ;
LDF 14(R2),AC0 ;
JSR PC,CVFX ;
LDF 30(R2),AC0 ;
JSR PC,CVFX ;
LDF 44(R2),AC0 ;
JSR PC,CVFX ;
MOV #OUTBUF,R0 ;
JSR PC,@4(SP) ;
ADD #4,R2 ;Next row
SOB R3,6$ ;
MOV #CRLFX,R0 ;
JSR PC,@4(SP) ;
MOV #OUTBUF,R0 ;
MOV #3,R3 ;Now do the 0 0 0 1 row
7$: CLRF AC0
JSR PC,CVFX ;
SOB R3,7$
LDF ONE,AC0
JSR PC,CVFX
MOV #OUTBUF,R0 ;
JSR PC,@4(SP) ;
MOV (SP)+,R3 ;Restore R3
BR 3$ ;Go to the exit stage
CVFX: ;Version of CVF that saves R1.
MOV R1,-(SP) ;
JSR PC,CVF ;
MOV (SP)+,R1 ;
RTS PC ;
DATA
SNAME: ASCIE /SCALAR /
VNAME: ASCIE /VECTOR /
TNAME: ASCIE /TRANS /
CODE
; BREAK, NOOP, TOPAL
.IFZ ALAID
BREAK: ;Interpreter routine
MOV #BRKMES,R0 ;
JSR PC,TYPSTR ;
BPT ;Cause a DDT break
CCC ;Clear condition code
RTS PC ;Done
DATA
BRKMES: ASCIE </
PROGRAM BREAK/>
CODE
.ENDC
NOOP: ;Interpreter routine
CCC ;Clear condition code
RTS PC ;Done
TOPAL: ;Interpreter routine
COMMENT ⊗ Escape to PAL. JSRs to the pseudo code. That code
should return via:
MOV PC,R0
RTS PC
⊗
JSR PC,@IPC(R4) ;Fly
ADD #2,R0 ;R0 ← Proper new IPC
MOV R0,IPC(R4) ;Hope R4, R3 still OK!
RTS PC ;Done.
;Initialization psops: PROG, ENDP, FIXIT
PROG:
COMMENT ⊗ Sets up the variables for each arm, with the associated
calculators. This is done by using some special-purpose pseudo-code
and setting this interpreter to work on it. There is one argument,
which is the version number of the pcode. ⊗
MOV IPC(R4),-(SP) ;Save the IPC.
MOV #PROGCD,IPC(R4) ;Set up a funny IPC
MOV RF,-(SP) ;Save RF
MOV SP,RF
JSR PC,INTERP ;Call ourselves to execute the code.
MOV (SP)+,IPC(R4) ;Restore the IPC
CCC ;Clear condition code
RTS PC ;Done
DATA
PROGCD:
XMVAR ;Make the mechanism variables
.IFNZ YELLOW
YAOFST
YHOFST
.ENDC
BAOFST
BHOFST
BDEPROACH ;Make the deproach variables
YDEPROACH
0
.IFNZ YELLOW
XMEXP ;The expression for updating the YARM
0 ; no neededs (so not dependent on the mechanism)
PCDYA ; code
YACOFS ; offset of expression
XMCLC ;Make it a calculator
YACOFS ; offset of expression
YAOFST ; offset of variable
XMEXP ;The expression for updating the YHAND
0 ; no neededs (so not dependent on the mechanism)
PCDYH ; code
YHCOFS ; offset of expression
XMCLC ;Make it a calculator
YHCOFS ; offset of expression
YHOFST ; offset of variable
.ENDC
XMEXP ;The expression for updating the BARM
0 ; no neededs (so not dependent on the mechanism)
PCDBA ; code
BACOFS ; offset of expression
XMCLC ;Make it a calculator
BACOFS ; offset of expression
BAOFST ; offset of variable
XMEXP ;The expression for updating the BHAND
0 ; no neededs (so not dependent on the mechanism)
PCDBH ; code
BHCOFS ; offset of expression
XMCLC ;Make it a calculator
BHCOFS ; offset of expression
BHOFST ; offset of variable
XPUSH ;Put some junk on the stack
0 ;
XENDCLC ;Returns to caller, and clears the stack
.IFNZ YELLOW
;PCDYA: XWHERE ;Expression for YARM
YARM ;
XENDCLC ;
;PCDYH: XWHERE ;Expression for YHAND
YHAND ;
XENDCLC ;
.ENDC
PCDBA: XWHERE ;Expression for BARM
BARM ;
XENDCLC ;
PCDBH: XWHERE ;Expression for BHAND
BHAND ;
XENDCLC ;
CODE
ENDP:
COMMENT ⊗ Cleans up the variables for each arm, with the associated
calculators. This is done by using some special-purpose pseudo-code
and setting this interpreter to work on it. ⊗
MOV IPC(R4),-(SP) ;Save the IPC.
MOV #ENDPCD,IPC(R4) ;Set up a funny IPC
MOV RF,-(SP) ;Save RF
MOV SP,RF
JSR PC,INTERP ;Call ourselves to execute the code.
MOV (SP)+,IPC(R4) ;Restore the IPC
CCC ;Clear condition code
JMP TERMINATE ;Done with the interpreter
DATA
ENDPCD:
XKVAR ;Kill the mechanism variables
.IFNZ YELLOW
YAOFST
YHOFST
.ENDC
BAOFST
BHOFST
BDEPROACH ;Kill the deproach variables
YDEPROACH
0
XPUSH ;Put some junk on the stack
0 ;
XENDCLC ;Returns to caller, and clears the stack
CODE
FIXIT:
COMMENT ⊗ This should only have to be called from DDT. Unwedges the
servos. ⊗
MOV #34,R0 ;
JSR PC,GTFREE ;Get a device block
MOV R0,-(SP) ;
MOV R0,R1 ;
JSR PC,@LINTARM ;Initialize all servos
TST R0 ;All well?
BEQ 1$ ;Yes
MOV R0,-(SP) ;No
MOV #FIXM,R0 ;Complain.
JSR PC,TYPSTR ; without getting back into DDT prematurely
MOV (SP)+,R0 ;
JSR PC,TYPOCT ;
1$: MOV (SP)+,R0 ;
JSR PC,RLFREE ;Reclaim the device block
RTS PC ;
DATA
FIXM: ASCIE </
CAN'T INITIALIZE ARM. ERROR CODE = />
CODE
;BUGS
COMMENT ⊗
Any variables (like FORCE variables, CMONS, or ordinary declared
variables) inside the conclusion of a CMON use level-offsets later
used for other things. Fix: Let the conclusion of a CMON be at a new
lexical level. Changes to PASS3: Trivial. Changes to INTERP: Make
CMTRIG do the nasty work. (DONE changes made to PASS3 & CMMAK - ARG 11/76)
⊗